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ABSTRACT 


Although robotics research has produced a wealth of sophisticated control and sens- 
ing algorithms, very little research has been aimed at reliably combining these con- 
trol and sensing strategies so that a specific task can be executed. To improve 
the reliability of robotic systems, analytic techniques are developed for calculating ] 

the probability that a particular combination of control and sensing algorithms will 
satisfy the required specifications. The probability can then be used to assess the 
reliability of the design. An entropy formulation is first used to quickly eliminate 
designs not capable of meeting the specifications. Next, a framework for analyzing 
reliability based on the first order second moment methods of structural engineering 
is proposed. To ensure performance over an interval of time, lower bounds on the 
reliability of meeting a set of quadratic specifications with a Gaussian discrete time 
invariant control system are derived. A case study analyzing visual positioning in a 
robotic system is considered. The reliability of meeting timing and positioning spec- 1 

ifications in the presence of camera pixel truncation, forward and inverse kinematic 
errors, and Gaussian joint measurement noise is determined. This information is 
used to select a visual sensing strategy, a kinematic algorithm, and a discrete com- 
pensator capable of accomplishing the desired task. Simulation results using PUMA 
560 kinematic and dynamic characteristics are presented. 
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1. INTRODUCTION 


As the study of robotic systems has progressed throughout the years, a large archive 
of special purpose algorithms suitable for various control and sensing tasks has been 
produced. Often several different algorithms are available which are capable of 
executing the same task with varying levels of performance. Use of redundant mea- 
surements, for example, improves accuracy at the cost of increased computational 
time. Similarly, a vision system may be capable of producing depth measurements 
using feature matching, point matching, focusing, structured lighting, stadimetry, 
etc. Control strategies may include either a PD or PID compensator for the same 
task, thus yielding different accuracies and response times. Classically designed com- 
pensators may be well suited for meeting a desired settling time and overshoot, while 
optimally designed compensators are best at meeting a quadratic cost functional. 

1.1 Motivation 

Although many robotic control and sensing algorithms have been derived, very 
little effort has been directed at ensuring and analyzing the reliability of a robotic 
plan which uses the algorithms to accomplish a specific task. As a consequence, 
it is quite possible to program an elaborate sequence of events using sophisticated 
control and sensing algorithms, but no formal methods have been developed for 
guaranteeing that the sequence will reliably produce the desired effect. 

In light of the plethora of algorithms available for treating many common 
robotic tasks, this work does not derive new control/sensing algorithms, but rather 
presents a consistent and general framework for analyzing the reliability of these 
algorithms. As such, it contributes to the final goal of designing ’’intelligent ma- 
chines” capable of operating in uncertain environments with minimal supervision or 
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interaction with a human operator. Other aspects of this subject have already been 
extensively -developed by Saridis, who has proposed the combination of artificial in- 
telligence, control systems, and operations research through the use of information 
theory [45], [60], [48]. 

Until recently, reliability efforts were confined for the most part to empirical 
“common sense engineering” approaches. For instance, to reliably grasp a compo- 
nent, a series of fixtures were fashioned until the component could be held securely 
in a specified position. After repeated experimental successes, the plan was deemed 
reliable. This method is time consuming and hard to generalize. Recently, sev- 
eral works have analyzed the reliability of particular sensing algorithms, but have 
still not suggested methods for combining sets of algorithms and assessing reliabil- 
ity. Unfortunately, increased precision is often gained at the cost of increased time, 
computations, complexity, etc. Consequently, a sensing algorithm often cannot be 
fully evaluated in itself, but must be considered in conjunction with the control 
system which produces actions based on the measured perceptions. Similarly, the 
control performance depends on the sensing statistics. In short, control and sensing 
are dual entities: control determines interaction with the environment while sensing 
observes the effect of these interactions. 

1.2 Problem Definition 

Given an explicit task to be executed by the intelligent machine and a set of 
plans, .4 = {,4j, . . . , A n }, consisting of control and sensing strategies applicable to 
the task, first select those plans which are potentially capable of attaining perfor- 
mance within the desired specifications (feasible plans). This selection procedure is 
a coarse and computationally efficient stochastic analysis aimed at greatly reducing 
the number of plans for which the explicit probability of success must be calculated. 
For these feasible plans (.4/ eaJi ). find the reliabilities associated with the alternative 
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subsets of control and sensing algorithms such that the task can be accomplished to 
meet the set of desired specifications, So = {si, . . . , s m }. 

At this juncture, the reliability of hardware components (power supplies, pro- 
cessors, sensors, actuators, etc.) is much greater than the reliability of plans to 
execute tasks. Therefore, hardware failures will be neglected, and the analysis will 
concentrate on planning faults. 

1.3 Overview of the Approach 

This work proposes methods for combining control and sensing within the 
context of robotic systems. Since concepts from reliability theory, control theory, 
and sensor fusion are used, we have coined the term “reliable control and sensor 
fusion” [33] to describe this research field. Reliable control and sensor fusion is 
defined as the unification of sensor information and control strategies such that an 
acceptable level of reliability in accomplishing a desired task is achieved. 

Rather than relying on heuristic simplifications or highly constrained tasks, a 
general mathematical framework founded on entropy is derived. First, it is shown 
that reliability specifications can be mapped to a set of entropy constraints which 
define the precision necessary for the task at hand. Next, an algorithm which makes 
use of these entropy constraints is presented for selecting feasible sets of control 
and sensing algorithms for a given task. Since entropy is invariant with respect 
to homogeneous coordinate frame transformations [28], this approach simplifies the 
reliability analysis for physically distributed robotic systems. In addition, by using 
reliability and information theory, techniques are developed to provide an analytical 
means of evaluating the reliability of a sequence of elementary events. Moreover, 
by augmenting reliability theory with information theoretic concepts, simple meth- 
ods of combining the reliabilities of different coordinator sub-systems are obtained. 
To ensure performance over an interval of time, lower bounds on the reliability of 


meeting a set of quadratic specifications with a Gaussian discrete time invariant 
control system are derived. These concepts are validated via a detailed analysis of 
an extremely common robotic task-the problem of vision guided positioning to an 
oriented point. 

1.4 Literature Review 

Previous research on individual issues of reliability analysis in intelligent ma- 
chines has contributed greatly to certain segments of this work. In particular, six- 
issues relevant to this analysis are well trodden research areas. 

1.4.1 Generation of Reliable Plans 

The previous attempts to generate reliable robotic plans can be divided into 
two categories. The first school of thought attempts production of reliable plans 
by modeling the environment (using either probabilistic techniques or worst case 
geometric bounds) and, based on the model, generating a plan robust enough to 
reliably execute the task without errors. This thesis takes a similar approach from 
a stochastic viewpoint, but concentrates on analysis which will facilitate planning, 
rather than considering planning directly. The second school emphasizes error iden- 
tification and recovery, with the assumption that reliability may be achieved by 
recovering from most errors. A considerable body of work has been dedicated to 
this topic, but most works do not pertain to this thesis. Since this thesis uses a 
stochastic approach, only the stochastic attempts at error identification and recov- 
ery will be reviewed. ls ... 

Brooks [6] models the environment using a geometric analysis of tolerances 
rather than using stochastic techniques. In particular, geometric bounds on the al- 
lowable position error for a peg-in-hole insertion problem are symbolically derived. 
Since a symbolic expression is available, it is possible to alter geometric parameters 
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-and see the effect on the bounds. The bound information is incorporated into an AI 
based u plan checker” which checks the bounds against the positioning constraints to 
ensure that the plan will reliably be executed. The main advantage of the method 
js that the symbolic expressions allow alternative plans to be investigated. Unfortu- 
nately, the symbolic expressions do become very complex even for simple geometric 
arrangements. In addition, the geometric approach tends to check only the worst 
case conditions, and thus resulting plans may be quite conservative. 

Mazon and Alami [31] are also principally concerned with improving relia- 
bility in the presence of positioning uncertainty, but they make use of stochastic 
methods of modeling the position uncertainty. The uncertainty is modeled as a zeio 
mean Gaussian disturbance to a homogeneous coordinate frame. The disturbance 
is propagated between homogeneous coordinate systems in the form of covariance 
statistics. The covariance is transformed between homogeneous coordinate frames 
using rotation and translation matrices. The uncertainty information is used off-line 
to build a flexible plan which uses a LISP based program to check the build up of 
errors and compare them to those allowed by the fixtures. If the error is too large, 
a search is used to find a strategy which has a greater chance of success. 

Havel and Kramosil [15] also use a stochastic approach to robot plan formation 
for reliability improvement. The planning strategy is rigorously defined using formal 
language techniques. Based upon reliability estimates, searches for reliable plans are 
performed. The reliability functions are not obtained through a stochastic analysis, 
but instead are heuristically defined. For instance, one measure of reliability is 
defined such that it is inversely proportional to the number of iterations required 

for success. 

Henderson [IT] implicitly strives for reliability by attempting to organize the 
knowledge necessary to map robotic system requirements onto an appropriate as- 
sembly of algorithms, processors, sensors, and actuators. Synthesized systems which 
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are kw, to operate reliably are packaged as “ logica l sensors” and are then a vaikMe 
as a system resource. 

Smith and Gini [53] propose reliability improvement through error analysis 
and recovery. A set of sensors are used to trace the execution of the robot” and 
detect when an error has occurred. The nature of the sensors used in this detection 
are not specified. Once the cause of the error is determined, the plan is reformulated 

to allow “forward recovery” rather than attempting to return to the state prior to 
the error (“backward recovery”). 

Taylor and Taylor [581 also investigate error identification and recovery, but 
concentrate their work on error identification. The goal is the minimization of 
sensor processing during identification. All possible errors are enumerated, and the 
probability of each error is used. The paper does not discnsrmeans of obtaining 
the error vector or the probabilities. Based on this information, two dual problem! 
are treated. First, estimates of the probability that a particular sensor will detect 
a particular error source are found. Next, methods of determining the error source 

given sensor readings are developed. In a later paper [59], Bayes theorem is used to 
determine the error source. 


1-4.2 Reliability Analysis Techniques 

Reliability analysis techniques for calculating the probability of success of large 
systems with algebraic relationships have been developed by civil engineers for use 

in analyzing structural safety. These techniques have recently been extended for use 
in manufacturing analysis. 

Shinozuka [49] reviews several structural reliability analysis techniques. Three 
methods are recommended: Monte Carlo simulation, use of the Stoke and Gauss 
divergence theorem to reduce the dimensionality of the problem, and first order 
second moment methods. The first order second moment methods are treated in 
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detail, and it is shown that the reliability index is the point most likely to fail. 

Parkinson develops the first order second moment methods in a series of pa- 
pers. First, the technique is derived in terms of correlated Gaussian variates, rather 
than uncorrelated standard normal variates [36]. In addition, an iterative method 
of calculating the reliability index is proposed. Next, the first four sample moments 
are used, along with the Johnson transformation, to obtain a multivariable standard 
normal distribution corresponding to the sample data [38]. For each measurement, 
x,, a corresponding standard normal variate, z,, is found. These marginally nor- 
mal variates are assumed to be jointly normal. The covariance between transformed 
sample data points is found numerically. The statistical uncertainty in the reliability 
index caused by the sampling is estimated for two special cases-linear and spherical 
performance functions. A Bayesian based technique is suggested for updating the 
estimates if additional data is given. 

The iterative technique of solving for the reliability index is further developed 
in [37], In particular, by numerically evaluating the failure surface at a number 
of points, an algorithm for categorizing the failure surface as linear or spherical is 
proposed. Based on the categorization, the reliability may be calculated using either 
the normal or chi-squared distribution functions. 

Parkinson applies the first order second moment method to assembly of man- 
ufactured components with given tolerance ranges [39]. In this case, a failure has 
occurred if the set of components will not assemble correctly. The first four sam- 
ple moments and the Johnson transform are employed to obtain a set of standard 
normal variates for use in calculating the reliability index. A numerical example is 
applied to assembly of a bicycle crank. 

Lee and Woo [30] continue Parkinson’s application in manufacturing toler- 
ances. Through use of Parkinson’s reliability analysis of tolerances, methods of 
tolerance selection are found. The resulting solution employs integer programming 


methods to optimize tolerance choice. 


1.4.3 Stochastic Sensor Fusion 

Determining the accuracy of robotic sensors holds some unique problems in 
that the sensor systems are often distributed physically over several different lo- 
cations which may be changed. To address this issue, Smith and Cheeseman [52] 
propose a method of transforming the covariance statistics of three degree of free- 
dom position measurements between homogeneous coordinate frames. The method 
makes use of the 3x6 Jacobian matrix between the frames, and Kalman filtering is 
employed to combine the measurements once they are transformed to a common 
coordinate system. 

Durrant-Whyte [8] [10] extends the Jacobian based propagation of covariance 
to the full six degrees of freedom. The analysis demonstrates that orientation er- 
rors in an initial frame can cause position errors in the new frame as a function 
of the distance between frames. In addition, a technique is presented of describing 
geometric features (i.e. points, lines, planes, circles, etc.) as families of parame- 
terized functions with probability distributions defined on the parameter vectors. 
These features (represented as parameter vectors) can then be transformed between 
coordinate frames. The methods are used to develop maximal information sensing 
strategies. 

Durrant-Whyte [9] extends these ideas to develop a theory for integrating and 
propagating geometric sensor observations throughout a distributed system. The 
result is a two step process. First, the measurements are integrated into the system 
representation. Next, the measurements are propagated throughout the system 
to maintain consistency. Bayesian methods are used to combine measurements, 
with both the jointly normal and “contaminated Gaussian” distributions treated in 
detail. Differential homogeneous transforms are used to consistently integrate closed 


9 


kinematic chains of measurements. 

1.4.4 Reliability Analysis of Individual Algorithms 

Several particular algorithms for sensing and control have been analyzed for 
accuracy and reliability. For instance, Azadivar [3] analyzes the effect of joint posi- 
tioning errors on hand accuracy using a stochastic approach, rather than bounding 
volumes. The errors are assumed to be zero mean, and are assumed to arise solely 
from random joint positioning. In other words, link and joint flexibilities, as well 
as link parameter errors are neglected. A “success function” is defined which is a 
function of hand position, desired hand position, and a vector of cost measures. 
The probability of success is then found by the sample mean of the success function. 
The method is applied to a pin-in-hole example. A rudimentary form of feedback 
analysis is incorporated by assuming that the uncertainty can be reduced, through 
feedback, by a multiplicative factor p (p < 1) each r seconds. 

Considerable research has been focused recently on analysis of stereo vision 
systems. Blostein and Huang [4] analyze the accuracy of two parallel cameras in 
measuring the position of a point. The quantization caused by discrete pixels is 
assumed to be the sole source of error, i.e. calibration and focusing imperfections 
are neglected. The quantization of the pixels in each image plane produce a volume 
of uncertainty corresponding to each pair of left and right camera pixels. It is 
assumed that the point’s actual position is uniformly distributed within the volume 
of uncertainty. Based on this assumption, the corresponding distribution in the 
image plane is derived. Using this information, the probability that the relative 
range error is within a tolerance bound is calculated as a function of the disparity. 

Rodriguez and Aggarwal [42] continue Blostein and Huang’s analysis, but make 
one different assumption. Rather than assuming that the position of the point is 


uniformly distributed in the volume of uncertainty, the analysis is simplified by as- 
suming that the image position of the point is contaminated by uniformly distributed 
noise. This greatly simplifies the calculations at the cost of decreased model fidelity. 
The range error probability density is derived in terms of camera parameters. 

For tractability, Lee and Kay [29] linearize the error analysis of a stereo vision 
system with parallel cameras. Two sources of error are considered: errors arising 
from Gaussian image noise, and positioning errors of the vision system. In contrast 
to previous papers, the analysis includes all three degrees of freedom for a point 
(not just range). Moreover, four points arranged like a cube and its vertices are 
examined simultaneously. As a consequence, both position and orientation errors 
of an object are determined. A Kalman filtering scheme is employed to combine a 
sequence of images for use in motion estimation. 

Because the correspondence problems in stereo vision pose great difficulties, 
Grandjean and Robert de Saint Vincent [12] merge data from both a laser range 
finder and a stereo vision system. An extended Kalman filter is used to fuse low 
level information into higher level constructs (i.e. points to lines; lines to planes; 
etc.). 

Rather than stochastically modeling the sources of error, Han [13] improves 
the reliability of computation using homogeneous transformations by incorporating 
fault tolerant linear arithmetic coding. By using various checksums, both error 
detection and recovery in homogeneous transforms can be realized. The method is 
especially applicable to parallel processing systems because a series of homogeneous 
transformations may be performed simultaneously on individual processors, and 
then transmitted to a central processor. In this manner, the fault tolerant scheme 
offers one method of compensating for communication errors." 
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1.4.5 Entropy Based System Analysis and Design 

To analyze the information flows in large systems, entropy has been used by 
several researchers. Conant [7] models hierarchical general systems in this manner. 
A “partition law of information rates” is derived which shows that requirements 
on a system for selection of appropriate information, coordination of parts, and 
throughput are additive. In a related paper, Koomen [25] uses entropy to model 
the process of design. The model is based on Conant’s partition law of information 
rates. 

Saridis and Valavanis [48] make use of Conant’s partition law for analytically 
designing intelligent machines. The intelligent machine is formulated as the mathe- 
matical problem of finding the right sequence of internal decisions and controls for 
a system structured in the order of intelligence and inverse order of precision such 
that it minimizes its total entropy. 

Entropy concepts are used by Sanderson [44] for modeling the assembly of 
manufacturing parts constrained to a number of discrete positions. The model is 
useful for assessing product designs and quantifying the complexity of assembly 
procedures. Sensors are incorporated through use of conditional entropy. 

In a series of papers, Kalata and Priemer derive entropy based stochastic 
approximation algorithms. A minimax error entropy stochastic approximation al- 
gorithm is used to estimate the state of a non-linear discrete time system in [22]. 
In addition, an upper bound formula for the resulting error entropy is presented. 
The upper bound is further employed in [24] to determine when smoothing should 
cease. The error entropy method is also used for system identification both with 
and without uncertainty [21]. The ideas are further clarified in [23]. 


1.4.6 Reliability Analysis of Control Systems 

A large body of research exists within control theory for designing controls 
which are reliable in responding to uncertain plant parameters or actuator failures. 
Typically, a “reliable” control is defined to be a control law which remains stable 
even when the worst case uncertainty occurs. The control development in this thesis, 
on the other hand, analyzes the ability of a given control law in meeting a set of 
quadratic specifications when the system is perturbed by zero mean Gaussian noise. 
Hence the thrust is on analysis, not design, of controls. Moreover, reliability is 
defined as the probability of meeting a set of specifications. 

In order to design control systems which are stable in the presence of failures, 
Viswanadham’s book [62] provides detailed information. In a later work, Viswanad- 
ham [61] concentrates on fault detection and diagnosis in automated manufacturing 
systems. The approach is based on Petri nets. 

In a series of papers ([43], [54], [40], [55], [41]) Stengel and Ray develop a 
procedure for estimating the stochastic robustness of a linear time-invariant system. 
Given a probability distribution for the uncertainties, a Monte Carlo simulation is 
performed to find the probability that the system will become unstable. The method 
can be extended to find probabilities for system charac ter istics other than stability. 

A method for finding the class of all compensators for linear time invariant 
systems which will produce a desired steady state covariance is developed by Skel- 
ton , Hsieh, and Ikeda ([18], [50]). Since methods are developed for designing the 
stochastic parameters of the system, it may be possible to combine the reliability 
analysis techniques developed in this work with Skelton's design techniques. Further 
background information is available in [51]. 
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1.5 Organization of the Thesis 

This "thesis suggests a two-stage reliability analysis. The first stage allows 
rapid selection of feasible plans by using entropy constraints and is presented in 
chapter 2. The second stage, which is outlined in chapter 3, proposes methods for 
calculating the probabilities of success for each feasible plan. To ensure performance 
over an interval of time, lower bounds on the reliability of meeting a set of quadratic 
specifications with a Gaussian discrete time invariant control system are derived 
in chapter 4. To test these concepts on problems of realistic difficulty, chapter 5 
applies the methodology to visual positioning using a stadimetric vision system and 
a six degree of freedom manipulator. Finally, chapter 6 discusses the work and its 
contributions. 



2. ENTROPY BASED SELECTION OF FEASIBLE PLANS 

In order to synthesize control and sensing with reliability, a two step analysis is 
proposed. First, a set of entropy constraints is found which must be satisfied for re- 
liable operation to occur. Those sets of control and sensing algorithms which satisfy 
the entropy constraints will form feasible sets of algorithms. For many applications, 
satisfaction of the entropy constraints alone will yield sufficient reliability for the 
task at hand. This is especially true when failure imposes only a small penalty. On 
the other hand, when a very high degree of reliability is required, a second stage of 
analysis which explicitly calculates the reliability (/?,-) corresponding to each feasible 
subset (Af eaSi ) is necessary and is explained in the next chapter. 

This thesis develops a new entropy based technique which allows efficient se- 
lection of feasible plans. The more intelligent activities required to formulate the 
sets of algorithms are not considered. Rather, the statistical behavior of a given 
plan is explored and evaluated. Thus (in the context of Intelligent Machines as 
proposed by Valavanis and Saridis [60]) the Organizer and Coordinators first select, 
from the library of all possible control and sensing algorithms, subsets of algorithms 
potentially able to solve the given task. These subsets may well contain multiple 
control algorithms for applications such as gross positioning followed by fine move- 
ment. Moreover, multiple sensing algorithms may be included for utilizing several 
sensor subsystems. A particular subset is denoted as A,, and may be regarded as a 
low level plan for executing the desired task. On the other hand, not all algorithms 
are included because some obviously do not apply. For instance, if position mea- 
surement is required, vision routines for object inventory are not included. For the 
most part, this portion of the planning utilizes logical predicates which are adroitly 
handled through the use of Petri net transducers [63], In contrast, once these plans 
are formulated, it is still necessary to examine the statistical characteristics of the 
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plans, A{, and their relationship to the set of desired specifications, Sp. 

The objective of this chapter is: select from the entire space of plans A = 
{Ai, . . . , .4 n } those subsets of algorithms that may reasonably be expected to attain 
performance within the desired specifications. These plans will be called the feasible 
plans, Aj ea ,. This goal is achieved through an information theoretic interpretation 
of the desired specifications using Jaynes maximum entropy method (MEM). 

2.1 An Entropy Formulation of Desired Specifications 

Jaynes maximum entropy method is a technique for determining the least bi- 
ased probability distribution of a probability space subject to given constraints. The 
least biased distribution is that distribution which maximizes the entropy subject 
to the given constraints [19], [20]. The MEM has been applied to many statistical 
inference questions, and several classes of problems have useful and tractable solu- 
tions. For instance, suppose we wish to find the density, /(x), of a random variable 
x subject to the condition that the expected values, of n known functions g t ( x) 
are given. MEM analysis yields a density 

f{x) = Aexp[-Ai$i(x) Kg n {x)} (2.1) 

where A, A x , • • • , A„ are constants derived from the constraints [35], The maximum 
entropy corresponding to the constraints is 


H{x) = + • • • + A n /z n - In. 4 (2.2) 

Consequently, if constraints involving expected values (i.e. moments, etc.) are 
known, then the MEM, through (2.1) and (2.2), provides the least biased distribution 
and entropy. 

MEM distributions and entropies for several common sets of constraints are 
presented in [14] and [23]. For convenience, Table 2.1 summarizes those results. 


Constraints 

Distribution 

Density 

Entropy 

Bounds, (a, 6) 

Uniform 

1 

b—a 

ln[6 — a ] 

Mean (n), Variance (a 2 ) 

Gaussian 

I f (X-Jl)- 1 ! 

y ln[2~e<7 2 ] 

7f?7 eX Pl 2<r5 J 

Bounds (a, b), 
Mean g. 

Truncated 

Exponential 

c — Ao - Afc C ; 

„ 1 be~ Xb -ae~ Xa 

\ g — \q — -V6 

ln[~- a ~y r _u ] 


Table 2.1: Maximum Entropy distributions and entropies for several 
common sets of constraints 


The key to unifying the reliability and information theoretic approaches lies 
in the MEM of Jaynes. Given the set of desired specifications, Sp, it is possible 
using the MEM to generate the least biased probability density function given the 
specifications. Moreover, the maximum entropy corresponding to the specifications 
is also provided by the method. For instance, if the specification is a safe velocity 
range, (— v max , v mar ), then the MEM yields a uniform distribution as the maximum 
entropy distribution, with .//(specification) = ln2u max (Table 2.1). Similarly, if 
the specification is a positioning error with mean of zero and variance cr 2 p , then 
the maximum entropy distribution is Gaussian with //(specification) = In ^J'2-ea^ 
The method is especially adept at modeling hard bounds due to sensor or actuator 
saturation. For instance, a desired force mean subject to force sensor saturation can 
be modeled with the truncated exponential distribution. 

In effect, the set of specifications (Sp) include into the design an allowable 
level of uncertainty. The MEM facilitates the expression of this design uncertainty 
as a Shannon entropy. Once formulated in this manner, several important concepts 
from information theory may be invoked. For instance, information theorists have 
long noted that entropy is analogous to information. Consequently, the specifica- 
tions needed for reliable operation may, through MEM analysis, be incorporated 
into information flows within an Intelligent Machine. Moreover, Saridis [48] defines 
knowledge as a form of structured information. As a result, the knowledge embodied 
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in the design specifications can be mathematically represented through the infor- 
mation thepretic approach. A method for utilizing this knowledge as a criteria for 
reliable control and sensor fusion is presented in the next section. 

2.2 Selection of Feasible Plans 

The entropy formulation of the specifications allows well developed concepts 
from information theory and intelligent control theory to be combined and utilized 
in the plan selection. By propagating the entropies of distributed sensor readings 
to the control coordinate frame and fusing these entropies with that of the con- 
troller, the total entropy //(control, sensing) can be compared to the maximum 
entropy allowed by the specifications, //(specification). If //(control, sensing) ex- 
ceeds //(specification), then that set of sensing and control may not be capable of 
reliably meeting the specification, and will therefore not be included in the feasible 

algorithms, Aj eas = {Aj eaSl , .A/ eaJJ , . . . , A/ easi }. This induces a formal definition for 
feasible plans: 

Definition: Given an explicit task to be executed, and a subset of control 
and sensing algorithms, A, = (C,, 5,}, corresponding to that task, A, is 
a feasible plan (denoted by Aj eaSi ) if the entropy constraints 

H{A, k )<H{s k ) (2.3) 

are satisfied for all of the specifications, s kl k = H{A l ) is the 

entropy of .4, in responding to the k th specification, and H{s k ) is the 
uncertainty embodied in the k th specification. 

This definition yields a set of entropy constraints which must be satisfied to ensure 
reliable operation. Thus once all entropies have been determined, finding those 
plans which are feasible can be accomplished in a straightforward manner from the 
entropy constraints as depicted in Figure 2.1. 



Figure 2.1: A Flowchart for Selecting Feasible Plans. 
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Intuitively, equation (2.3) implies that the uncertainty regarding a feasible 
subset’s ability to meet each design criteria must be less than the permissible level 
of uncertainty embodied in each design specification. To be more concrete, consider 
what is perhaps the most common design scenario- a specification consisting of a 
tolerance range (a, b) and a feasible plan whose response is A /■%*> N([a + b]/2,a 2 ). 
This may represent a positioning tolerance, a desired force and safe bounds, velocity 
constraints, etc. For. feasibility, (2.3) and Table 2.1 imply that 

In x/2~ecr 2 < ln[6 — a] (2.4) 


or, equivalently 


4.13 a < b — a 


(2.5) 


For this problem, the reliability is the probability that A stays within the tolerance 
range, i.e. 

R = Pr{a < A < 6} (2.6) 


Since the response distribution is Gaussian, the reliability is easily found: 

' b-a 


R = 2$ 


2cr 


- 1 


(2.7) 


where $(.) is the standard normal cumulative distribution function. The worst 
case reliability allowed by (2.5) is 4.13cr = b — a which produces R > 0.96. Thus 
the entropy constraints (2.3) yield at least 0.96 reliability for Gaussian responses 
subject to tolerance specifications. Consequently, the implicit reliability contained 
in the entropy constraints is sufficient to ensure satisfactory performance for this 
example. Lower bounds of the reliability implied by satisfaction of (2.3) for a class 
of distributions subject to tolerance specifications are derived in Appendix A. 

Satisfying the set of entropy inequalities puts the plans through a threshold- 
ing process. As long as the thresholds are met, then a lower bound reliability is 
guaranteed for tolerance (Appendix A) or quadratic (Section 4.3) specifications. In 


addition to the thresholding process, it is possible to order the plans according to 
their entropy levels. The plans with the least entropy can then be analyzed first by 
the reliability analysis stage. The analysis is then considered complete when a plan 
of sufficient reliability is found. This approach is very similar to the entropy based 
planning in Saridis and Valavanis’s Hierarchically Intelligent Machine [46]. 

Note that the mean of the plan and that of the specification are identical for 
the examples considered. This ensures that the plan uncertainty and the specifica- 
tion uncertainty pertain to the same physical parameters; without this property, the 
unmodified entropy constraints can allow less reliable plans to be regarded as feasi- 
ble. Typically, the desired specifications require that the mean value of the plan’s 
response is equal to a particular value. In this case, if the mean of the plan is not 
equal to the mean of the specification, then the desired specification is not satisfied. 
Thus the plan is immediately excluded from the feasible set. One exception to this 
rule is the specification consisting only of bounds, [a, b]. Since a plan’s response is 
acceptable if it occurs anywhere within the bounds, the plan’s mean could conceiv- 
ably be anywhere within the bounds while maintaining reliable operation. Reliable 
entropy constraints can be found by mapping the desired specification to a new 
specification which has a mean equal to the plan mean. The new specification can 
be found by retaining the specification bound which the response mean is closest to 
and matching means. For instance, suppose the i th plan’s mean in responding to 
the j th specification (consisting of bounds [a, 6]) is . Since the MEM indicates 
that the uniform distribution corresponds to this specification (Table 2.1), the mean 
of the specification is (a + b)j 2. If > (a + b)/ 2, then the upper bound of the 
specification, 6, is preserved. Next, the mean of the new specification is made equal 
to the mean of the plan. If [a', 6'] denotes the new specification, then this procedure 
results in two equations 
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a! -f b' 
2 


Pa„ 


(2.9) 


These two equations are solved simultaneously to produce the new specification, 
[a', 6'] = [2 ha, } — b, b}. Similarly, if / i Aij < (a +b)/ 2, then the new specification is 
[o', 6'] = [a,2fiA,j — a]- The technique is applied in the case study to the timing 
specification. 


2.3 Advantages of the Information Theoretic Approach 

The information theoretic approach to reliable control and sensor fusion holds 
several advantages over other methods of analysis. These advantages can be divided 
into three categories. First, the entropy associated with many sensed features useful 
in robotics is invariant with respect to coordinate frame transformations. Second, 
since the MEM is a well developed tool of statistical inference, it provides exact 
methods of handling uncertainty for any distribution. Third, because entropy can 
be interpreted as information, it provides a consistent representation throughout all 
levels in a hierarchically Intelligent Machine. 

First, consider entropy in the presence of coordinate transformation. The 
joint entropy of many diverse types of measurements are invariant with respect to 
coordinate frame transformations [28]. This is easily shown by noting that the joint 
entropy of a random vector x j = g( i,) is found by [35]: 

H{xj) = H{x t ) + £{ln \det(J)\) (2.10) 

where J is the Jacobian of the uniquely invertible transformation g{-), J = V x £(;r). 
For coordinate transformations, = g(x,) = T-x, ( [l l] ) where x } is a point rep- 
resented in the j th coordinate frame, x, is a point represented in the i th frame, 
and 


n o a q 


1 

>} 

0 0 0 1 


0 1 


V = 


( 2 . 11 ) 
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where n, o, a, and q are vectors, and R is an orthonormal rotation matrix. The 
Jacobian o_f this transformation is simply J = T , ] . Since R is orthonormal, det(J) = 
det(R) *1 = 1. Substitution into (2.10) then yields H{ xj) = H(x,). This fact 
was first noted by Kyriakopoulos [28]. Kyriakopoulos also found that the result 
could be generalized far beyond mere 3-D points to include parameter vectors of 
several common geometric features: lines, planes, and spheres. In this formulation, 
a geometric feature such as a line is represented as a point (parameter vector) 
in parameter space following Durrant- Whyte [8]. The joint entropy of the entire 
parameter vector is then invariant with respect to coordinate frame transformations. 
The transformations for several useful entropy invariant parameter vectors are listed 


in Table 2.2. 

The method can also be extended to include six dimensional quantities such as 
oriented points in 3? 3 or force vectors. If p, = rj(p_,) represents the transformation 
of the oriented point p } in frame j to frame i, then J has a highly structured form 
[ 8 ]: 


j = j; = 


(2.12) 


R T 0 
M R T 

where R is the rotation matrix previously defined, the oriented points are represented 
as 6x1 vectors with the orientation stacked on top of the position, and 


M = 


qxn qxo qxa 


(2.13) 


This implies that det(J) = det 2 (R) = 1. Thus E{\n\det(J)\) = 0 and from (2.10) 
the joint entropy is again invariant with respect to coordinate frame transformations. 

This invariance with respect to coordinate frame transformations is very im- 
portant when comparing and combining sensor readings distributed over many dif- 
ferent coordinate frames- a very common situation in advanced robotic systems. To 
illustrate, cameras are often mounted on gimbals or on a link of the manipulator. 
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Transformation 
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P = (x y z i] T 

Pi = VPi 
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r(f) = r 0 4- td 

p = [d T Tq 1] t 
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0 T, j 

Px 

Plane 
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*-5 
I' II 

,k ---T2 
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Force 


P = [r F] T 

Pj = J) 1 Px 


Table 2.2: Features which display coordinate frame entropy invariance 


Similarly, mobile robots perceive the environment from constantly changing posi- 
tions. Even if the sensor itself is immobile, the relationship of the sensor with respect 
to the end effector’s coordinate frame will vary as the manipulator position changes. 
In addition, often several different sensors are distributed throughout the workcell. 
For instance, a laser range finder often complements a stereo vision system. The 
efficient fusion of such measurements is of great importance. 

Since modern robotic systems contain many sensors distributed over a vari- 
ety of different physical locations which may change with time, transforming these 
diverse sensor readings to a common coordinate frame has posed one of the fun- 
damental issues of sensor fusion research. Recently, attention has been focused on 
transforming both the sensed measurement and the uncertainty regarding that mea- 
surement, as the uncertainty may be used in the fusion of sensor readings. Most 
authors have expressed this uncertainty through the covariance matrix [8], [52]. 
A more compact and useful representation of the uncertainty is Shannon’s entropy. 



Shannon’s definition of entropy has gained wide acceptance because it complies with 
a heuristic .understanding of uncertainty and provides a mathematical framework to 
express this uncertainty [35]. Moreover, entropy can be used in estimation to replace 
covariance statistics [23], [22]. 

In contrast, the covariance matrix varies under coordinate frame transforma- 
tions. Propagation of the covariance matrix can be approximately calculated as 
[ 8 ]: 

Cj = jjC t jf (2.14) 

"'here C 3 is the covariance in the j th frame, J/ is the Jacobian matrix of the coor- 
dinate transformation defined in equation (2.12), and C, is the covariance in the i th 
frame. As a result, in order to evaluate the performance of a distributed sensor in 
the control frame using covariance analysis, it is first necessary to calculate the Ja- 
cobian between those frames, and then find the propagated covariance using (2.14). 
In contrast, since the joint entropy is invariant with respect to coordinate frame 
transformations, it is not necessary to find the Jacobian when using entropv based 
analysis. Because many different measurements are often available at a variety of 
coordinate frames, this simplification makes comparison between alternative sensing 
strategies much more computationally tractable. In all fairness, it must be stated 
that the complete covariance matrix is sometimes a useful quantity to be propagated 
in its own right. This is especially true when the desired specifications concern in- 
dividual elements of the feature’s parameter vector. However, even in these cases, 
the information theoretic approach still holds several important advantages. 

To be explicit, the information theoretic approach is advantageous because it is 
a well developed statistical inference technique. As a result, the wealth of previous 
knowledge contained in information theory literature may be immediately drawn 
upon. For instance, the entropy formulation expresses uncertainty in a meaningful 
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fashion for any probability distribution. In contrast, propagating merely the covari- 
ance is sufficient in expressing the uncertainty only for certain distributions such as 
those listed in [23]. Other distributions require higher order moments for complete 
analysis of the uncertainty. In addition, Kalata [23] notes that 

A significant result obtained from the characterization of communication 
systems using information theory is that performance bounds can be es- 
tablished independently of coding procedures. A similar result is possible 
when information theory is applied to the general estimation problem: 
independent of the estimation procedure, a performance bound can be 
specified. 

Thus, when fusing sensor measurements, the entropy scheme allows determination 
of the estimation performance bounds. This knowledge is useful for determining 
sensing strategies of sufficient accuracy. 

The final advantage which the unification of reliability and entropy concepts 
presents is the consistent representation of uncertainty throughout all levels of a 
hierarchically Intelligent Machine. Since entropy can be regarded as information, 
it is a sufficient analytic measure that unifies the treatment of all the levels of an 
intelligent machine [48]. This implies, for instance, that, in an impasse, the higher 
level Organizer may re-design the problem, thus generating new specifications for 
the lower level reliable control and sensor fusion. An iterative design procedure is 
then possible in the spirit of Koomen [26]. In addition, since entropies are produced 
for each feasible subset and each specification, the techniques have great potential for 
integration with other well established results of Intelligent Control. For instance, 
if a positioning specification is phrased in terms of a maximum integral error, then 
it may be possible to make use of entropy formulations of optimal control [45]. 



3. RELIABILITY ANALYSIS OF FEASIBLE PLANS 


When a high degree of reliability must be assured, a second stage of analysis which 
explicitly calculates the reliability ( R ,) corresponding to each feasible subset (A/ eaJi ) 
is necessary in addition to the entropy based elimination procedure. Just as the 
specifications play an integral role in the determination of feasible plans, Sp also 
greatly influences R t . The set of specifications may include constraints on total ex- 
ecution time, positioning accuracy, maximum overshoot, robustness, tracking errors 
etc. Each element of the specifications, s*, represents a desired characteristic to 
be achieved during task execution, while R t incorporates all of these criteria into a 
single term and measures the probability that they will all be satisfied. This mea- 
surement is accomplished by first calculating the reliability of the plan in responding 
to each separate specification {R,k, k = l,...,m). Next, the individual reliability 
terms are combined to form R t . 

3.1 Calculation of Reliability Terms 

The individual reliability terms (R,k) can be found by first defining reliabil- 
ity performance functions (RPF), g t k, associated with each feasible subset. Aj ea s,, 
and each specification, Sk ■ Often, this is the most arduous phase of the reliability 
analysis, as it requires capturing the probabilistic behavior of a particular algorithm 
and expressing it in terms of the desired specifications. The performance functions 
should be defined in such a manner that 


(x) > 0] =£• success 

(3.1) 

[5>fc(z) < 0] => failure 

(3.2) 


where x is a vector of state variables. 


Once each RPF is defined, the statistics associated with the current state of 
the environment can be used to find the reliability. R lk , of the particular subset 
Af eaSl in meeting the desired specification Sk- If the form of the underlying dis- 
tribution for the state variables is known (based on prior experience), while the 
distribution parameters must be found from statistical sampling, then maximum 
likelihood estimation may be applied. 

Maximum likelihood estimation is useful because under certain regularity con- 
ditions it possesses several compelling advantages. The regularity conditions are 
not very restrictive. In short, if samples (x^ X 2 , x*} are taken from an underly- 
ing cumulative distribution function P(x;e), where e is a distribution parameter to 
be estimated, then the maximum likelihood estimate of e, e, meets the regularity 
conditions if £(x;e) is regular with respect to its first two e derivatives (directional 
derivative) and e is unique. £(x;e) is regular with respect to its first e derivative if 



£{S(x;e)} = ^/~d£(x;e) = 0 

(3.3) 

where 

Q 

S{x\e) = — log dP{x\e) 

(3.4) 

P(x ; e) is 

regular with respect to its second e derivative if 



£{S'(x; e)} + £{S(x; e)} 2 = ^ dP(x; e) = 0 

(3.5) 

w’here 

3 

S'{x\e) = — 5(x;e) 

(3.6) 


Consult [64] or [65] for a rigorous treatment of maximum likelihood estimation. 
Under these conditions, the maximum likelihood estimate (MLE) is asymptotically 
normal, consistent, and asymptotically efficient. Furthermore, for a function of e, 
/(e), the MLE is given by the invariance property to be /(e) [65]. The distribution 
of the function is 

(3.7) 


/(e) - ,4A'(/(e). V? f(e)Cov(e)VJ(e)) 
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where 

• /(e) is the MLE of /(e), 

• ,-LV indicates asymptotic normality, 

• V e /(e) is the gradient of /(e) with respect to e, and 

• Cov(e) is the covariance matrix of e. 

Consequently, if the regularity conditions are satisfied for the underlying dis- 
tribution, then from the sample ensemble {x!,X 2 , x*} it is possible by maximizing 
the likelihood and using (3.7) to find an asymptotically normal maximum likelihood 
distribution for g,k(x)- Since the distribution is asymptotically normal, it is a simple 
matter to then find the reliability corresponding to the RPF. 

If maximum likelihood estimation cannot be used, a lower bound on R may 
be found from the reliability index. A lower bound is extremely useful, as it pro- 
vides a guaranteed minimum level of reliability. The disadvantage, of course, is 
that the lower bound can be a conservative estimate of the actual reliability. The 
reliability index , (3, is defined as the minimum distance between the origin of a set 
of uncorrelated standard normal variates (derived from x), and the failure surface . 
g{x) = 0. 

Consider the case in which the individual state variables, denoted now as x,, 
are uncorrelated, Gaussian random variables. The variables can be replaced by a 
set of reduced variates with the transformation: 



where p Xi is the expected value of the i th random variable, and <7 Xi is the standard 
deviation of the i th random variable. To estimate the reliability, Shinozuka [49] has 
shown that the point on the failure surface (#(x) = 0) with the minimum distance to 
the origin of the reduced variates, x', is the most probable failure point. If g(x) is a 
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1 
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nonlinear function, the reliability index may be used as an approximate measure of 
reliability by placing a lower bound on the reliability. For linear g(x), the reliability 
can be found exactly from the reliability index. 

Determining the reliability index, 0, may require iterative methods for nonlin- 
ear performance functions. In contrast, linear performance functions have a closed 
form solution. Suppose that the performance function is represented as: 


g(x) = a 0 -t- ^Ta.x, 


(3.9) 


where the a,’s are constants. Note that the reliability performance function may be 
negative, although this possibility corresponds to a system failure. The minimum 
distance to the origin of the reduced variates is then [l]: 


^ Qq ~h 


(3.10) 


For linear performance functions, the reliability can be found directly from the 
reliability index by the formula 

R = ${0) (3.11) 


where $(.) is the normalized, zero mean, Gaussian cumulative distribution function 
and R is the reliability. The minimum distance for nonlinear RPFs can be found by 
iteratively searching for the minimum of 

0 = -j^r— 

vWVs) 

subject to 

9 {x') = 0 (3.13) 

where Vg is the gradient vector of ^(x) with respect to x', and T denotes the 
transpose. Parkinson [37] offers a method for efficient solution to the minimization. 
If ^(x) is concave toward the origin of the reduced variates, then [39] 



xl(P) <R< *(&) 


(3.14) 



On the other hand, if g(x) is convex toward the origin, then 

W) < R < 1 (3.15) 

where \'n(-) ls chi-squared distribution function with n degrees of freedom. If 
the RPF is neither convex nor concave, (3.14) may always be used as a lower bound 
on the reliability. Figure 3.1 graphically depicts the bounds for a two dimensional 
problem. 

Should the variables be correlated, the solution is still possible. However, 
the covariance matrix must be used [49], For non-Gaussian variates, Ang [2] and 
Parkinson [39] explain techniques for transforming the variates to an equivalent 
Gaussian system. 

3.2 Combination of Reliability Terms 

Once the reliabilities in meeting each individual specification have been found, 
the ability of the potential algorithms {A JeaSi ) in meeting the desired specifications, 
So, depends on the relationships between the elements of So- In the majority of 
cases, the elements s* will have a series relationship, i.e. the success in meeting 
requirements will depend on all specifications being satisfied. Occasionally, the 
elements may have parallel relationships. For instance, it may be desired to either 
meet a desired overshoot, or a desired execution time. Since meeting either criteria 
is termed a success, the relationship is parallel. 

To allow ease in combining reliabilities, as well as to accommodate information 
flows within Intelligent Machines, a new concept is now defined. The reliability self 
information (RSI), /(/?), is 

I{R) = - log R 

, while the failure self information is 


(3.16) 


Geometries of the Reliability Index 



Figure 3.1: A Geometric Picture of the Reliability Index for Two Di 


mensions 



where F is the probability of failure, i.e. 


F = l - R (3.18) 

The base of the logarithm is arbitrary, as different bases will simply add a constant 
bias. Since these quantities are measures of self information, they enjoy many of the 
same properties which entropies do. For instance, multiplication can be replaced 
by addition, etc. The information theoretic setting makes RSI easy to interpret in 
terms of Intelligent Control as proposed by Saridis [48]. The flow of knowledge re- 
sulting from the reliability self information complies with the principle of Increasing 
Precision with Decreasing Intelligence [46]. 

Once the reliabilities R,k have been found, it is a simple matter to find the 
corresponding RSI’s, I(R,k) from (3.16). It is also a simple matter to find the RSI 
of the entire subset if its elements are in a series relationship. The reliability of a 
series system (assuming independence) is [2] 

R, = RiRf-Rn (3.19) 

where R x denotes the reliability of the i th component. The RSI for a potential subset 
of algorithms Aj eaSt corresponding to n specifications (s*) in a series relationship is 
found by combining (3.19) and (3.16): 

I(R,) = 'tl(R, 0 (3.20) 

fc=l 

Thus multiplication is replaced by addition when using RSI terms. On the other 
hand, if the set of specifications, So- contains parallel relationships, the parallel 
relationships must be simplified to one series term before (3.20) can be used. The 
reliability of a parallel arrangement (assuming independence) is: 
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Rewriting (3.21) in terms of failure probabilities, we find 

F = II F , (3-22) 

I 

The total failure self information of a parallel connection can be found from (3.17) 
and (3.22) 

1(F) = Y. HF.) (3.23) 

1 

As before, addition replaces multiplication. The RSI can be found from the failure 
self information by the simple formula 

I{R) = log ( 1 - exp [-/(F)]) (3.24) 

The RSI found from (3.24) and (3.23) is the equivalent RSI of the parallel set of 
specifications, so it can be combined with specifications in series with it using (3.20). 

Naturally, once the RSI, I{Ri), is found for each subset of feasible algorithms, 
Afgas , , the performance of the alternate plans can easily be compared. The smaller 
the RSI, the more reliable is the plan. In a more qualitative sense, the set of de- 
sired specifications operates on the feasible algorithms to produce a set of reliability 
performance functions. The environment then operates on the performance func- 
tions to produce a reliability self information (Figure 3.2). Figure 3.3 outlines the 
reliability analysis procedure in a flowchart form. 

To clarify these concepts further, the case study performs this analysis in detail 
for a robotic problem of practical significance. 








START 


Define: A feas = { A feasl , . . . , A /ea , n } 

= { 1 1 ■ • • i S m } 

RR F — {^ 1 1 ) • • • , 9nm } 




END 


V 



Find overall /(i? t ) by- 
combining series and 
parallel relations 
between I(Ri k ) terms. 


Determine j3 tk and R lk : 
Nonlinear: R > xl(3ik) 

Linear: R = P(0 tk ) 

I(R lk ) = -ln(R, k ) 


Figure 3.3: The Reliability Analysis Proced 














4. RELIABILITY ANALYSIS OF DISCRETE TIME-INVARIANT 


CONTROL SYSTEMS 

The reliability analysis techniques developed herein have focused on the types of 
specifications traditionally found in manufacturing environments, i.e. tolerance 
ranges, mean and variance criteria, etc. These specifications constrain principally 
the final state and extreme states, thus they can be met successfully with classical 
control tools such as steady state error and overshoot. By contrast, often the be- 
havior of a system must be ensured over a time segment. Moreover, in robotics it is 
very important to be able to specify the behavior of a multidimensional system. 

The performance of a system over a time interval is measured in optimal 
control theory as the value (termed cost ) of a functional defined over that interval 
(termed the performance function). If the performance function is defined to be the 
integral of a quadratic function of the state variables and the system is linear, then 
the control which maximizes performance (as defined by the performance function) 
can easily be found. 

By using nonlinear feedback, Tarn [57] has proved that robotic manipulators 
may be viewed as linear time invariant systems. Consequently, if a quadratic perfor- 
mance function is defined, then the optimal control for the i th sensing system can be 
found and the expected cost corresponding to that control and sensing strategy can 
be evaluated. Although the expected cost can be calculated, techniques have not 
been previously developed to determine the reliability of a control system in meet- 
ing a set of specifications. This chapter derives a lower bound reliability in meeting 
quadratic constraints on the error of discrete time invariant control systems. 
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4.1 Problem Statement 

The notation used will be very similar to that employed by Hsieh and Skelton 
[18], as the concepts developed here might be used in conjunction with their ideas 
to form a powerful design technique. 

The time invariant discrete plant is denoted 

x p (k + 1) = A p x p (k) -f B p u(k) + D p w(k) (4.1) 

*(&) = MpXp(k) + Eph-(k) (4.2) 

where x p 6 9? ni , u € 3? n “, z 6 are state, input, and measurement vectors, 
while w(k) € and v(k) € are zero mean white Gaussian noise sources with 
covariance matrices W(k) and V(k), respectively. The plant is controlled with a 
dynamic compensator of a specified order n c 

Xc{k + 1) = A c x c (k) + B c z(k) (4.3) 

u(k) = C c x c (k ) + D c z(k) (4.4) 

The initial condition of x p and x c is assumed to either be deterministically known 
or normally distributed. 

In order for operation of the system to be considered successful, the following 
set of weighted square norm specifications must be satisfied: 

Sk : x T (k)Q(k)x(k) < e 2 , k = l,...,A r (4.5) 

where x 6 9£ n p +ri ': and 

x = X ” (4.6) 

. x ‘ . 

The equations describing the evolution of x are, from (4. 1-4.4) 


x(k + 1) = A tt x(k ) + Dtivi(k) 


(4.7) 



where Vi(k) = [u, ,T (A:) i’ T (£)] T , 


A t , = 


A p + B p D c M p B p C c 
B C M P A c 


D tl = 


D p B p D c Ep 
0 B C E P 


(4.8) 


(4.9) 


4.2 Reliability Bound for Weighted Square Norms of Zero Mean Gaus- 
sian Vectors 

In order to assess the reliability of meeting the quadratic specifications (4.5), 
a general bound on the reliability of meeting a single weighted square norm will first 
be derived. This result is in itself useful, as many applications require the evaluation 
of only a single weighted norm. The analysis is performed by finding the reliability 
of meeting a very special weighted norm. 

Theorem 1 If x ~ N(0,C x ), x € 3?", then 

P{x T C; 1 x<e 7 }=xl(z 2 ) ( 4 - 10 ) 

where X^( - ) is the chi-squared cumulative distribution function with n degrees of 
freedom. 

Proof : 

Since C x is a covariance matrix, C" 1 is symmetric and positive definite. There- 
fore, it can be uniquely factored into the Cholesky decomposition. C" 1 = C T C [5]. 
Let y = Cx. Then 

E{y}= 0 (4.11) 

Cov(y) = CC X C T = C(C T C)-'C T (4.12) 

where £{•} denotes the expectation operator, and Cov{-) denotes covariance. Then 

Cov(y)C = C(C T C)~ l C T C = C (4.13) 
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Therefore Cov(y) = I, the nxn identity matrix. Linear transformations of Gaussian 
variates are also Gaussian [35], thus 

y ~ A r (0, 7) (4.14) 

The weighted norm can be rewritten 

* 7 C X = x T C T Cx = y T y = y\ + y\ + • ■ ■ + y^ = \ 2 (4.15) 

The elements of y are uncorrelated from (4.14), but this implies they are independent 
for Gaussian variates. Hence (4.15) is a sum of n squared, independent, standard 
normal variates. From [35], this implies that has a chi-squared distribution with 
n degrees of freedom. Hence 

P{x J C; x x = y T y < e 2 } = V (4.16) 

Thus if the weighting matrix is equal to the inverse of the covariance matrix, 
then the reliability can immediately and easily be found from tables of the chi- 
squared distribution. Of course, in general, the weighting matrix used to form 
a specification arises from physical needs and will not be related to the inverse 
covariance matrix. For instance, suppose that the vector x is a 6 dimensional pose 
error in a robotic system, with the first three elements representing the orientation 
and the last three elements representing the x,v,z position. The task at hand may 
require high positioning accuracy in the u x” direction, while the other directions 
and orientations are much less important. Then the weighting matrix, Q. should 
reflect this need for accuracy. Fortunately, Theorem 2 uses Theorem 1 to derive 
conditions under which a lower bound on reliability can easily be found for a class 
of weighting matrices Q. As such, Theorem 2 is a major contribution of this work. 

Theorem 2 If x ~ N(0,C x ), x € £R n , and C~ l — Q > 0 (the difference between 
C- 1 and Q is positive semi-definite ) then 


P{x T Qx<e 2 }>xl(e 2 ) 


(4.17) 
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Proof: 

The matrix Cj 1 — Q is positive semi-definite, therefore x T (C~ l — Q)x > 0 or 
x T C~ l x > x T Qx. By Theorem 1, P{x T C~ l x < e 2 } = y][(e 2 ). But if x is inside the 
hyperellipsoid defined by x T C~ l x < e 2 , then the inequality x T Qx < e 2 is also true. 
Thus 

P{x t Qx < e 2 } > P{i T C;'x < e 2 } > X 2 (e 2 ) -J (4.18) 

Theorem 2 is useful for analyzing many robotic tasks. For instance, suppose 
statistics for the error due to imperfect kinematic transformations are known. Then 
a lower bound on the probability of meeting a desired weighted square positioning 
norm can be found. 

4.3 Entropy Constraints for Selection of Feasible Plans 

Entropy has been proposed as a computationally efficient method of selecting 
feasible plans in a robotic system [34]. This section extends the technique to handle 
specifications consisting of a weighted square / 2 norm of a zero mean Gaussian 
random vector, and shows that the resulting entropy constraint yields a reliability 
which depends only on the dimension of the weighted vector. 

Feasible plans are defined in terms of a set of entropy inequalities via (2.3). 
This definition yields a set of entropy constraints which must be satisfied to ensure 
reliable operation. 

In order to use (2.3), the entropy of both the plan (simply a zero mean normal 
response for the case considered now) and the specification (a weighted square l 2 
norm) must be found. The specification entropy is found in accordance with [34] by 
using Jaynes Maximum Entropy Method. In order to find the maximum entropy 
distribution, Theorems 3 and 6 have been developed. 


Theorem 3 Given the constraint that a random variable x £ 3? n lies within the 
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/(*) = 


(4.19) 


hypersphere x T x < £ 2 , the Jaynes maximum entropy distribution is given by 

1 /K(e) , i T x < e 2 

0 , otherwise 

with entropy 

H(x) = \nV n (e) (4.20) 

where K,(e) is the hypervolume of a n-dimensional hypersphere of radius t. 

Proof: 

If f(x) and g(x) are two arbitrary probability densities, then [35] 

-[ g(x)\ng{x)dx < - f g(x)\nf(x)dx (4.21) 

J —o o j —CO 

Assume that 

( l/V n (e) , x T x < e 2 

/(*) = (4.22) 

[ 0 , otherwise 

then the entropy of x is 

*(/(*)) = , ln[V.( < )]- 1 /V.( e )&: = / <fx = ln[V„( e )] (4.23) 

*/r'x <« 2 V n\t) Jx~x<t 2 

Now suppose #(x) is any other density such that 

/ o(x)rfx = l (4.24) 

Jx 1 r <« 2 

Then, by (4.21), 

H{g{x)) = - f x T x<c i g{x)\ng(x)dx <-[ g(x) ln[V^(c)]” l dar 

Jx T x<t 2 


!x T x<i 2 

= In V n (e) / r T I<e 2 g{x)dx = H{f(x)) 


(4.25) 


Thus the entropy corresponding to any distribution other than f(x) is less than or 
equal to the entropy obtained by /(x), and /(x) is therefore the maximum entropy 
distribution, yj 

Theorem 3 can easily be generalized to hyperellipsoids, rather than spheres. 
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Lemma 4 Given the constraint that a random variable x £ lR n lies within the 
hyperellips'oid x T Qx < e 2 , Q > 0, the Jaynes maximum entropy distribution is given 
by 

1/Vn(e) , x T Qx < e 2 


f(x) = 


with entropy 


0 , otherwise 


H{x) = In V n (e) 


(4.26) 


(4.27) 


where V n (e) is the hypervolume of a n-dimensional hyperellipsoid of radius e, x T Qx < 


e 2 . 


The proof is identical to Theorem 3, except all integrals are taken over the hyper- 
volume x r Qx < e 2 


Lemma 5 Given two random vectors, x £ 9? n and y £ 3R n , subject to the con- 
straints, x T Qx < e 2 , y T Py < e 2 , Q > 0, P > 0, P — Q > 0, then 

H{y) < H(x) (4.28) ] 

where H(y) and H(x) are the entropies found from Jaynes MEM for y and x, 
respectively. 


Proof: 

Since both Q and P are positive semi-definite, x and y are constrained to be 
within hyperellipsoids. P — Q > 0 implies that x T Qx < x T Px < e 2 , thus the hyper- 
ellipsoid formed from the weighting matrix P is entirely within the hvperellipsoid 
formed by Q. This implies that the volume of Q' s hyperellipsoid is greater than or 
equal to the volume of P’s hyperellipsoid. From Lemma 4, H(y) < H(x) y/ 

Theorem 6 The hypervolume inside the hypersphere x T x < e 2 , x £ 9? n is given by 

V' n (e) = I\ n e n (4.29) 


1 
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t.- (n — l)A' n _ l A' n _ 2 ^ , 
A n = , n > 4 


rc/v n _3 


(4.30) 


<Z7l(f A', = 2, A' 2 = 7T, A 3 = 4tt /3 


Proof: 


(The approach to the proof was suggested by Kostas Kyriakopoulos.) For 
lines, discs and spheres, the volume is given by 


V^e) = 2e (line) 

^(O = j Vj (e cos #)ecos OdO = 7re 2 (disc) 

- r n 

I' 3 (c) = / V' r 2 (e cos0)ecos#d0 = 47rc 3 /3 (sphere) 

» — 7T /2 

In general, the volume of a hypersphere can be expressed as 

rir/2 

= / V),_ 1 (ecos5)ecos5(i^ 

J —T j 2 

Assume that V),_j can be written in the form 


v;_ 1 ( f ) = A ' n _ l€ ' 1 - 1 


Then 


/ *■ r* 

cos "WJ =» V n (e) = A' n e" 

•ir/2 ' ' 

Since Vj(€) = AV 1 , by induction V n [t) = K n e n . 

From a table of integrals, 


[ cos 71 OdO = - cos n 1 0 sin 0 + - — - f cos n ~ 2 
J n n J 


OdO 


(4.31) 

(4.32) 

(4.33) 


(4.34) 


(4.35) 


(4.36) 


(4.37) 


By combining (4.35), (4.36), and (4.37), a recursive formula for calculating the I\, 
coefficient can be found 


A'nC' 


r/ 2 

= / 

J-*/ 2 


cos" OdO 


(4.38) 
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r */2 K 

/ cos" Odd = n 
J — TT 


r/2 


An— 1 


n — 1 r! 2 
Tl J-tt/2 


cos" -2 Odd 


but 


Therefore 


L 


" /2 cos n ~ 2 ede = 

/2 An-3 


(n - l)A' n _l An-2 / 
A n = - — V 


(4.39) 


(4.40) 


(4.41) 


n A'n-3 

Theorems 3 and 6 are used along with Theorem 2 to find a lower bound on 
the reliability imposed by the definition of feasible plans. 


Theorem 7 Given the specification s : x T Qx < e 2 , x € 3fc n , x ~ A'(0,C x ), 1 — 

Q ^ 0, Q > 0 then the entropy constraint 


H{x) < //(spec) 


(4.42) 


(where //(spec) is the maximum entropy of x consistent with the specification si : 
x T C~ 1 x < e 2 ) yields a lower bound on reliability in meeting x T Qx < e 2 which 
depends only on the dimension of x. 


Proof: 

Let y — Cx where C is obtained from C" 1 = C T C (Cholesky decomposi- 
tion). From the proof of Theorem 1, y ~ A'(0,/). Since y has a standard normal 
distribution, its entropy is easily found [35] 

H(y) = ^ \n2~e (4.43) 

Because C arises from a covariance matrix, it is nonsingular. Thus H(y) can be 
expressed in terms of x using the formula [35] 

H(y) = H(Cx) = H(x) -I- In |de<(C)| (4.44) 

The specification Sj : x T Cf l x < e 2 can be rewritten as y T y < e 2 , therefore by 
Theorems 3 and 6, the maximum entropy of y subject to the constraint Si is 


H{spec y ) = In V' n (e) = In I\ n e n (4-45) 


■ 


I 


] 


i 


1 


Since H (spec) is the maximum entropy of x subject to the constraint s lt 

H(specy) = H(spec) + In \det(C)\ (4.46) 

As a consequence, the entropy constraint H(x) < H(spec) is equivalent to 


H(y) - In \det(C)\ < H(specy) — In \det(C)\ (4.47) 


or H(y) < H(speCy). From (4.45) and (4.43), the inequality is 


— In 27re < In I\ n e T 
2 ~ 


(4.48) 


Thus the squared norm threshold, e, must satisfy 

\f{ 2 jr e) 


e > 


(4.49) 


“ I<l /n 

in order for the entropy constraint to hold. Using Theorem 1, it is easy to calculate 
the reliability in satisfying s 1? i.e. 


P{x T c;\ < = xiK.’) 


(4.50) 


Since cumulative distribution functions are monotonically increasing, (4.49) and 
(4.50) imply that the reliability in meeting Si is at least x^(2ire/(A'^ n )]. Also, bv 
Theorem 2, the reliability in meeting s : x T Qx < e 2 is greater than or equal to the 
reliability in meeting s x . The worst case reliabilities in meeting s, given that the 
entropy constraint is met, are listed in Table 4.1. y/ 

Theorem 7 extends the techniques suggested in [34] to specifications consisting 
of weighted norms of Gaussian vectors, and provides a lower bound on reliability 
of plans selected as feasible. In robotics, 6 dimensional vectors are very common, 
as they can be used to describe the 6 degrees of freedom available in mechanical 
systems. The entropy constraint (4.42) yields a lower bound on reliability of 0.87 
for n = 6. Lower bounds for other dimensions are listed in Table 4.1. 



n 

I<n 

€ 

R-worst — X n ( £ ) 

i 

2 

2.07 

0.96 l| 

3 

4.19 

2.56 

0.91 

6 

5.2 

3.1 

0.87 

10 

2.6 

3.8 

0.83 

20 

0.025 

4.96 

0.78 

50 

1.7xl0 -13 

7.44 

0.72 

100 

2.4x10-'*° 

10.3 

0.68 


Table 4.1: Worst Case Reliabilities W'hen the Entropy Constraint is 

Satisfied 


4.4 Reliability of Meeting a Set of Quadratic Specifications 

The problem of finding the reliability in meeting the set of specifications 5 = 
{sj, s 2 , . . . , syv} given in (4.5) has not been solved exactly. Rather, a lower bound on 
the reliability has been found by employing Theorem 2 and the positive correlation 
between the specifications. 

Theorem 8 Given a discrete time system described by (4-1 - 4-4) 0T (4-V with the 
property Q — Aj t QA t i > 0 where Q > 0 is a constant matrix, then 


P{s k +i\sis 7 ...s k ) > P{s k ) 


(4.51) 


i.e., the specifications are positively correlated. 


Proof: 

The probability density function of x(k ) given that the specification at sample 
k has been satisfied is given by the continuous form of Bayes theorem to be [35] 


fx[k){x{k)\s k ) 


f slk) (x{k))P{s k H(i-) = 1(1-)) 

PM 


(4.52) 



where X(k) denotes the random value of x(k). But since X(k) is known to be within 
the hyperellipsoid, 


P(s k \X(k) = x(k)) = { 


1 , 

0 , 


x T {k)Qx{k) < £ 2 
x T (k)Qx{k) > e 2 


therefore 


fx(k){x{k)\s k ) 


fr { k)(x(k))/P(s k ) , x T (k)Qx(k) < e 2 

0 , x T {k)Qx{k) > e 2 


(4.53) 


(4.54) 


In other words, /r(*)(x(£)|s*) is concentrated entirely in the success region defined 
by x : T (k)Qx(k) < e 2 . Now divide x(k + 1) into two components, the component due 
to the previous state and the component attributed to noise, i.e. let 


x 3 (k + 1) = A tt x(k) 


(4.55) 


Xff(k + l) = Dtivi(k) (4.56) 

Since Q — Af ( QA t i > 0, x T (k)Qx(k) > x T (k)Aj t QA t ix(k). This implies that the 
weighted square norm of x s (k + 1) given s k is inside the success hyperellipsoid 
because 


x»{k + 1 ) T Qx 3 (k + 1) = x T (k)AjiQA ti x(k) < x T (k)Qx(k) < e 2 (4.57) 

Thus if noise is not present, then s k => Sj, j > k. The noise is white, therefore Vi(k) 
will not be affected by the fact that s k has occurred. Hence the conditional density 
x (k 4- 1 ) 1 5 * is given by 

fx(k+i){x(k + l)|sfc) = fx,(k+i){x 3 (k + l)\s k ) ® /xw(Jt+i)( I yv(^' + 1)) (4.58) 

where ® denotes n-fold convolution. The conditional density f Xl [k+i)(x 3 (k + l)|s*) 
is derived from (4.54) and (4.55), thus from (4.57) it is concentrated in the success 
hyperellipsoid. The density / rjv (*+i)(x ( v(fc + l)) is zero mean Gaussian since vi(k) and 



x(0) are Gaussian. As a result, the convolution of the densities, f x (k+i)(x(k + l)|s*.) 
is more concentrated in the success region than f x ( k+X )(x(k + 1)). Hence 

-P(si + i|3*) = / fx{k+i)[z{k + l)|s*..)dx(/: + 1) > P(sk+ 1 ) (4.59) 

In addition, since vi is zero mean white Gaussian noise and the system constantly 
reduces the weighted square norm of the previous state, P(s k+1 |s 1 3 2 . . . s k ) > P(s k ) 

V 

Positive correlation of the specifications can be used to find a lower bound 
reliability for (4.5). 

Theorem 9 Given a system defined by (4-1 - 4-4) or (4- 7), with state covariance 
matrix C x ^) = £'{i(A:)x T (A:)} and a set of specifications 

s k : x T (k)Qx(k) < e 2 , k = 1 ,. . . ,N (4.60) 

where — Q > 0, k = 1, . . . , N, Q > 0, and Q — A^ t QA t i > 0 the reliability of 
meeting the specifications (4-60) has a lower bound of 

R>lxl(t 2 )f (4.61) 

Proof: 

Since C~^ — Q > 0 and x(£) is a zero mean normal variable, by Theorem 2 

P(s k ) > xl(e 2 ) (4.62) 

Bv the chain rule of probabilities, 

R = P{s x S 2 • • ■ 5A- ) = P{s x )P(s 2 \s x )P{s 3 \s x S 2 ) • • • P{s : \'\s x S 2 - - ■ SA'_l) (4.63) 
Theorem 8 shows that P(s*+i|siS 2 . ..s k ) > P{s k ). Therefore 

R>P(si)P(st)---PM 


(4.64) 
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From (4.62), this implies that 

R > V (4.65) 

In order to use Theorem 9, the closed loop system must satisfy the constraint 

Q — AjiQAti > 0 (4.66) 

One method of ensuring that (4.66) is satisfied involves first picking an arbitrary 
matrix Q x € 3? nXn such that Q x > 0. A weighting matrix Q can then be found by 
numerically solving the algebraic Lyapunov equation 

Q ~ AjiQA t i = Q l (4.67) 

for a positive definite real symmetric matrix Q. Such a solution exists for (4.67) as 
long as x(k+l) = A t ,x(k ) is asymptotically stable [27], In the case study simulation, 
Qi is chosen as Q x = kl, where A: is a positive integer scaling the identity matrix. 

The Q matrix found from (4.67) does not, of course, have any relationship to 
the specifications. Lemma 10 allows Q to be related to the specifications. 

Lemma 10 Given a system defined by (4-1 - 4-4) or (4- 7i> with state covariance 
matrix C x ( t) = E {x(k)x T (k)} and a set of specifications 

s k : x T (k)Q d (k)x(k) < e 2 , k = 1, . . . , N (4.68) 

where C;( k) - Q d (k) > 0, k = 1, . . . , N, Q - Aj t QA tl >0,Q>0, Q d {k ) > 0, Q - 
Qd{k) > 0, k = 1, . . . , N the reliability of meeting the specifications (4-68) has a 
lower bound of 

R > [xt(< ! )r (4.69) 

Proof: 

Since Q-Aj t QA t i > 0, the system monotonically moves into the hyperellipsoid 
defined by x T (k)Qx(k)e' 2 . From Q - Q d (k) > 0, k = if the specification 
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s ?Jk : x T [k)Qx(k) < e 2 is satisfied, then s*, '■ x T (k)Qd(k)x(k) < e 2 is also satisfied. 
As a consequence, the system also moves into the hyperellipsoids x T (k)Q ti (k)x(k) < 
e 2 . The s*.- specifications are then positively correlated (Theorem 8). Since C”^j — 
Qd{k) > 0, k = 1 ,. . . , N, Theorem 2 yields 

P{*k) > Xl(t 2 ) (4-70) 


The positive correlation between specifications then implies 


R > bcltf)]" V 


(4.71) 


Thus as long as the Q matrix found by solving the Lyapunov equation (4.67) 
satisfies Q — Qd{k), k — 1, . . . , N, then the reliability of hyperellipsoid specifications 
described by Qd{k) can be found. Note that this allows treatment of time varying 
specifications. 

A second lemma treats the case in which the specifications constrain only a 
portion of the state variables, obtaining an improved bound over that found directly 
by Lemma 10. 


Lemma 11 Given the conditions of Lemma 10 except C ~, k , — Qd(k ) > 0. k = 


A\ if 


Qd{k) = 


Qiu b{k) 0 
0 0 


(4.72) 


and 


C~ l uk(fc) - Q, ub (k) >Q,k = l,...,N (4.73) 

where Q sub {k) 6 3? rna ' m , m < n, x sub denotes the first m elements of x, and 
is the covariance matrix of x, ub (k), then 




R > (X^e 2 )] 


(4.74) 


s 


I 


] 


1 


Proof: 


1 
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From Lemma 10, the specifications are positively correlated. The specifications 
can be rewritten as 

^k ■ X sub(^)Q sub{k)x sub{k) ^ C (4-/5) 

Since x(k) is normally distributed with zero mean, x S ub(k) is also. From Theorem 
2, P($k) > Xm( f2 )- The positive correlation then yields the lower bound 

(4.76) 

The results of this lemma are not, of course, restricted to only the first elements of 
the state vector, x. Rather, by rearranging the vector, any subset of the state vector 
may be used. 

Lemma 11 is used extensively during the case study, because the case study 
has specifications constraining the 6 dimensional pose error of a robotic system. The 
state vector for the system, on the other hand, has 24 states. In order to optimize 
the design, the case study makes use of the following result from discrete Lyapunov 
control theory [27], 

Theorem 12 Given the discrete time invariant system 

x(k + 1) = A t x(k) + B t u{k) (4.77) 

with A t asymptotically stable and a real positive definite symmetric weighting matrix 
Q > 0 such that Q - AjQA t > 0, the feedback 

u m (k) = -( BjQB t )~ x BjQA t x{k ) = -K t x(k) (4.78) 

reduces the weighted square norm x T (k)Qx(k) at the optimal rate. 

Proof: 

First define a discrete Lyapunov function 


T'(x(*)) = x T (k)Qx(k) 


(4.79) 



Since Q > 0, by the Stability theorem of Lyapunov, the system is asymptotically 
stable in the large if 

A V[x{k)) = V(x{k + 1)) - V(x(*)) < 0 (4.80) 

Let the feedback be denoted as u(k) = —I\tx(k). Then 

AV'(x(£)) = x T {k)[A t — B t I\i] T Q[At — B t K[]x(k) — x T (k)Qx{k) (4.81) 


or 


A V(x(k)) — x T (k)AjQA t x(k)+2x T (k)Aj QB t u(k)+u T (k)B?Q B t u(k)—x T (k)Qx(k) 

(4.82) 

In order to find the control, u(k), which will reduce the weighted square norm of 
x(k) at its optimal rate, AV(x(k)) must be mini m ized. Minimization with respect 
to u(k ) requires that 

5AV(x(jfc)) 


du(k) 


= 0 = 2BjQA t x(k) + 2 BjQB t u{k) 


=» u m (k) - -{Bj Q B t )~ l Bj Q A t x{k) 

This does in fact produce a minimum because 

d& 2 v(x(k)) „ nTAn „ 

v v n = P - 2 BjQB t > 0 


(4.83) 

(4.84) 

(4.85) 


du{k ) 2 

i.e. P is positive semi-definite, so a minimum has been reached. The positive semi- 
definiteness of 2 BjQB t follows from the fact that Q > 0 =>• z T Q: > 0, z ^ 0. But - 
can be written as : = \/2B t y. Then z T Q: = y T 2BjQB t y > 0, z ^ 0. Since B t may 
have a nonzero null space, z may equal zero when y 7 = 0. Thus y T Py > 0, y ^ 0. yj 
Theorem 12 is not a new result of this work. Rather, it is a well known result 
from discrete Lyapunov control theory applied to the results of this work. 

In order to design a control algorithm for 


x[k + 1) = A t x(k) + B t u(k) + D t v(k) 


(4.86) 
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where E{x(k)} = 0 (this implies that only the stochastic errors are penalized, 
not deterministic transient effects in the control law), v(k) is zero mean uncorre- 
lated white Gaussian noise, x(k + 1) = A t x(k) is asymptotically stable, and the 
controller must be capable of reliably satisfying a set of specifications of the form 
Sk : x T (k)Qti(k)x(k) = xJ ub {k)Q sub (k)x 3Xlb (k) < e 2 , the following algorithm is sug- 
gested: 

1. Select a matrix Qi > 0. 

2. Find a real positive definite solution Q to the Lyapunov equation Q — Aj Q A t = 
Q i numerically. 

3. If Q - Qd(k) > 0, V* = 1, . . . , N, then let u(k) = BjQA t x(k) to 

produce a new (optimized) system x(k + 1) = A tt x(k ) + D tl vi{k). 

4. Calculate the covariance of x(k), C x (k), by solving C x (k + 1) = A t iC x {k)Aj t + 
D t iCov{vi{k))Dj t . 

5. Obtain the covariance matrix, C, ub (k) = Cov[x sub {k)), for the subset of the 
state variables which are constrained by the specifications. C sub (k) can be 
found from C x (k). 

6* K Csubik) — Qsub(k) > 0, V k = 1, . . . , N then R > [Xm( e2 )] A - Otherwise, try 
selecting a different Q x or changing A t through inner loop feedback gains and 
go back to step 1. 

This design procedure is used in the case study for reliably controlling a high pre- 
cision positioning task in a robotic system. 

4.5 Conclusions 

The techniques developed here provide a method of easily analyzing specifica- 
tions posed as a quadratic function of zero mean normal random vectors. This is 
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useful in itself for evaluating static errors in robotic systems such as those arising 
from an imperfect model of the manipulator kinematics or from a noisy set-point 
measured by a vision system. In addition, the method is applicable to multivariable 
discrete time invariant control systems. Thus noise inside the feedback loop (such as 
encoder noise) can also be analyzed, and the reliability of staying within a hyperel- 
lipsoid defined by the quadratic specifications over an interval of time can be found. 
In addition, entropy constraints are developed for the quadratic specifications, and 
are shown to yield a lower bound on reliability which depends only on the order of 
the vector. 

The method has great potential for use in conjunction with Skelton’s “co- 
variance control” [18]. Skelton finds a parameterization for all compensators of a 
specified order which will achieve a desired steady state covariance (for linear time 
invariant systems). Skelton does not, on the other hand, develop any methods 
for specifying the desired covariance matrix. Using the concepts developed herein, 
it is possible to first phrase the specifications in terms of physically meaningful 
weighted norms. A steady state covariance matrix C x can then be selected such 
that C~ l - Q > 0, and a compensator can be found which obtains C x using Skel- 
ton's theorems. Theorem 9 can then be used to find a lower bound on the reliability 
of meeting the quadratic specifications over a time interval. 


\ 


I 


5. CASE STUDY-RELIABILITY OF VISUAL POSITIONING 


Visual positioning is an extremely important and highly developed facet of robotics. 
However, surprisingly few attempts have been made at analyzing the reliability of 
visual positioning. Thus, this section has a two-fold purpose. First, it illustrates 
the ideas of the preceding chapters so that the notation and concepts are made 
clear. Second, it provides a general reliability solution, useful for many hand/eve 
coordination tasks of a robotic manipulator. 

Problem to be Solved: A manipulator has already moved close to a grip- 
ping post on an object using a priori knowledge. Due to environmental 
uncertainties, the final movement cannot be completed without measur- 
ing the current pose of the gripping post. Using a vision system mounted 
on a separate arm, it is possible to view the object from N different pre- 
programmed positions. Five inverse kinematics routines are available, 
and a library of M computed torque compensators can be used for ma- 
nipulator control. It is desired to make the final movement subject to 
two specifications-the total execution time must be less than t j and the 
gripper must not collide with the gripping post. Find reliable subsets of 
control and sensing which are capable of satisfying these specifications. 

The first step in the analysis is the definition of the desired set of specifications, 
Sq- To avoid collision with the gripping post, error tolerances are generated in each 
of the 6 degrees of freedom from the geometry of the gripper and gripping post. 
Hyperellipsoids are then inscribed within these tolerance bounds as a function of 
the distance from the final gripping frame. The geometry of the post and gripper is 
illustrated in Figures 5.1 and 5.2, while the overall configuration of the case study is 
shown in Figure 5.3. The homogeneous transformation from the initial end effector 
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frame to the base of the manipulating arm is 


- 'barm 


100 o 

0 0-1 -.3 
0 1 0 -.2 
0 0 0 1 


( 5 . 1 ) 


The transform from the nominal gripping frame to the base of the manipulator is 


n-ibarm 

1 grip “ 


10 0 0 
0 0-1 -.5 
0 1 0 -.2 
0 0 0 1 


( 5 . 2 ) 


To properly grip the post, the end effector frame should be driven to coincide with 
the gripping frame. Thus the end effector must be moved 20cm along its "z” axis 
for the nominal grip to be performed. The object is not at its nominal position at 
all times, rather the nominal position is a noisy estimate of the object’s actual pose. 
Consequently, a measurement of the object’s pose must be made with the vision 
system, and then the measured transformation from the gripping frame to the end 
effector frame is used to generate a joint trajectory capable of docking the gripper 
with the gripping post. 

To perform the docking reliably, the generated trajectory must prevent colli- 
sions between the gripper and post. These collisions can be modeled by defining 
tolerance bounds in each direction. The tolerance bound in the end effector’s 4i x” 
direction is easy to define because a collision will not occur due to the errors in this 
direction. The only fear is that the grip will not be centered. To prevent drastically 
off-centered grips, a tolerance bound of 3cm is defined. The tolerance value in the 
end effector’s “y” direction, on the other hand, is more critical as errors in u y” will 


59 


cause collisions. Using the data in Figure 5.1, the “y v tolerance is defined to be 

•9/ 3 , p, < 2/4 + I3 

e ymax ~ < P; — 2/4 — I3 + .9/5 , 2/4 + ^3 < Pz < 2/4 + I3 — .9/5 T fg (b- 3 ) 

As , Pz > 2/4 + A} _ - 9 /o + ^6 

where is the absolute value of the distance to be moved along the end effector’s 

tt z” axis to dock the gripping and end effector origins. Since the specification be- 
comes increasingly tighter as the object is approached, the end effector is, in effect, 
“funneled” into the gripping pose. Once the gripping post is inside a portion of the 
gripper, the “y” tolerance reaches its minimum value and stays at that constant 
value for the remainder of the docking. 

The specification for the u z” direction also becomes increasingly tighter as the 
object is approached. It is defined to be 

Pz + A} 1 Pz < U 

h 1 U S Pz < 2/4 + 13 ( 5 - 4 ) 

Pz Aj 7 Pz ^ 2/4 -f- I3 

In contrast to the “y” specification, the “z” specification does not reach a minimum 
before the docking is complete, but instead becomes increasingly difficult to meet. 
This reflects the fact that, as the gripping post is moved farther into the gripper, 
a collision between the inside end of the gripper and the post is more difficult to 
avoid. 



The tolerance bounds for orientation are coupled to the allowable errors in 
positioning. For instance, if a large error is present in the u y” direction, then even 
slight rotations about the end effector’s “x" axis will produce a collision. The u x” 
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orientation tolerance is thus defined as 
- arctan 

p z — (2/4 + / 3 ) + arctan 

^Qjrnax \ 


P : < 2/4 + h 

2 1 4 + l 3 < Pz < tt/2 + 2/4 + / 3 


— arctan .5 / 5 // 4 


( tt/ 2 , pj > tt/ 2 -f- 2/4 + f 3 — arctan .0/5//4 

(5.5) 

The “y” orientation error is similar to the “x” positioning error- it will not cause 
collisions under reasonable circumstances, but will cause off-centered grips. Thus 
e« vm „ = 23 degrees. Finally, rotational errors about the end effector’s ‘‘z” axis can 
cause collisions of the edge of the gripper with the post, thus 


C B , 


where 


= < 


a , Pz < 2/4 + l 3 

Pz — (2/4 + / 3 ) + o . , 2/4 4- / 3 < Pz < tt /2 + 2/4 + / 3 — & ( 5 . 6 ) 

tt /2 ) Pz ^ 7r /2 + 2/4 + /3 — oc 


h~.U 5 f / // 
a = arccos —7 — arctan 1&/It 


y/Q + Z 


(5.7) 


The tolerance specifications obtained from the data in Figure 5.2 is plotted in Figures 
B.1-B.3. 


Once the tolerance specifications are found, it is a very simple matter to in- 
scribe hyperellipsoids inside those specifications. First, the radius of the hyperellip- 
soid, e, must be selected. If the conditions of Theorem 2 are met, the probability of 
staying inside the hyperellipsoid has a lower bound of *|(e 2 ). Thus t can be selected 
from tables of the chi-square distribution. For the simulation, the radius is selected 
to be e 2 = 22 which yields a lower bound probability of .9988. The pose error in the 
case study, e € is represented as the roll-pitch-yaw angles stacked on top of the 
position error. Thus the problem of inscribing hyperellipsoids can be expressed as 


1 


1 


1 


1 


finding a weighting matrix, Q px , such that if 

e T Q pr elet 2 


(5.8) 


I 
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is satisfied, then e is inside the tolerance hyperrectangle. 
One Q pi which satisfies these requirements is 


Qpx{Pz ) — 


S/e-l 

0 

0 

0 

0 

0 


0 

0 

0 

0 

0 


0 

0 

<7 e L 

0 

0 

0 


0 

0 

0 

0 

0 


0 

0 

0 

0 


e 2 /e 2 


yrnaz 


0 

0 

0 

0 

0 

9/2 


(5.9) 


Note that, since the tolerance specifications are a function of the distance to the 
gripping frame, p z , the weighting matrix (5.9) is also a function of p.. Since the 
distance from the gripping frame to the end effector ( p z ) is a function of the generated 
joint trajectory, ©<*(£), <? pr (-) can be viewed as a function of either ©<*(&) or k. 

By using the weighting matrix found from the geometry, the specifications for 
the given problem are: 


• si = { total execution time is less than a desired time, tj } 

• 52 = { e T (Ar)Q pr (/;)e(fc) < e 2 , for all of the time steps, k. present in the 
trajectory driving the end effector to the gripping frame } 


Next, the plans must be formed. In this case, each plan will consist of the triplet 
Ai = { V), A',, G ,}, where V), I\ i} and (?, are the vision algorithm, inverse kinematics, 
and compensator used by the i th plan, respectively. Each algorithm is now examined 
in depth to find expressions for the statistical performance of each plan. 


5.1 Analysis of the Vision System 

As previously stated, the vision system consists of a camera mounted on the 
end of a separate robot arm. Although the vision system may be positioned at N 
different viewpoints to reduce problems with noise, assume there is only sufficient 



TOP VIEW 



M h^-H 


FRONT VIEW 

Figure 5.4: The MSFC Target Used in the Vision System 

time to take measurements from one of the viewpoints. Each viewpoint will be 
regarded as a separate vision algorithm, thus the number of potential vision algo- 
rithms is also N. The accuracy in measuring the object’s pose will depend upon the 
pose of the object in the camera coordinate frame. Consequently, each viewpoint 
will have differing measurement statistics. These statistics can be found by using 
an approach similar to that of Lee and Kay [29]. 

The vision system used is based on a system developed by NASA’s Marshall 
Space Flight Center for autonomous docking of space ships. It consists of a single 
camera and a three dimensional target. The target is depicted in Figure 5.4. Since 
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zcam 



xcam 

Figure 5.5: A Pinhole Model of Camera Optics 


the target parameters (d x , d 2 , r) are known, it is possible to find the target’s pose 
with only a single camera. The target pose then defines the gripping frame’s pose 
because the transformation from the target to the gripping frame is known. The 
measurement is accomplished by first assigning a coordinate frame to the camera, 
as shown in Figure 5.5, such that the camera frame X-Y plane corresponds to the 
inverted image plane. Next, points are projected onto the image plane using the 
pin-hole model of camera optics to produce the inverted image frame points (x t , y t ) 
from the camera frame coordinates, p, = [p x , p y , p Zt l] T 


x, = 


fp r, 

Pz , + / 


(5.10) 
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Vi = 


fPy. 


(5.11) 


Pz t + / 

The positions of the target points with respect to the target coordinate system are 


-Jar — tar 


tar 

Pi ' 

= [0 

-di 

0 

1 ] T 

(5.12) 

tar 

P2 

= [0 

d , 

0 

1 ] 7 

(5.13) 

_ tar 

P3 

= [ d 2 

0 

0 

1 ] T 

(5.14) 

— tar 

Pa 

= [ 0 

0 

0 

1 1 T 

(5.15) 

> the ' 

centers 

of the 

circular retroreflectors. 

In the sim- 


ulation, the entire area of each circle is mapped onto the discrete grid formed by the 
CCD image plane. The centroid of that image is then found to determine the image 
position (i t , y,) which corresponds to the retroreflector center p\ ar . Unfortunately, 
if the circle is close to the image plane and at an angle with respect to the image 
plane, then the image plane’s centroid does not correspond to the center of the cir- 
cle. This slight bias does not pose a problem in the system under consideration, as 
will be demonstrated when the simulation results are presented. Let the unknown 
transformation from the target coordinate system to the camera coordinate system 
be denoted by 

f — p 


rpcam 

1 Car — 


pcam 

tar 


0 0 0 1 


(5.16) 


r>cam f„_ „ _1 
•ftfar = l nsa \ = 




Tly Sy Qy 


n * iS ^ g ~ 


( 5 . 1 ' 


P=\PrPyPz] T (5.18) 

The camera coordinates corresponding to the four target points are related by 

_cam rrcam „tar /- , n \ 

P, = * tar Pi (O- 19) 


1 
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These points are then substituted into (5.10, 5.11) to produce six nonlinear equations 
in nine unknowns 


x \{ — d\S : + Pz + f) = f{ — dis x + p x ) 

(5.20) 

y\{-d\S t + p z + /) = f(—d l s y +p y ) 

(5.21) 

X2{d\s z +Pi + /) = f[d\S x + p x ) 

(5.22) 

y 2 (dis z +p z + f) = f(dis y + p y ) 

(5.23) 

x 3 (d 2 n. +p.+ f) = f(d 2 n x + p x ) 

(5.24) 

y 3 (d 2 n z + p. + /) = f{d 2 n y + p y ) 

(5.25) 

Also, since i s orthonormal, 


n T n = 1 

(5.26) 

s T s = 1 

(5.27) 

n T s = 0 

(5.28) 

a = nxs 

(5.29) 


These nonlinear equations are solved in the simulation using the Newton-Raphson 
iterative technique for solving nonlinear simultaneous equations. The solution yields 
the transformation from the target coordinate system to the camera coordinate 
system, Tf™. Due to the iterative nature of the Newton-Raphson solution, the 
measurement time is stochastic. 

Once the homogeneous transformation matrix is found, it can be represented 
as a 6 x 1 vector by expressing the rotation matrix in terms of roll-pitch-yaw angles. 
Following the approach in [l 1 ] , the roll-pitch-yaw angles can be found as: 

9 X = atan2{n y ,n x ) (5.30) 

(5.31) 


9. = atan2(s‘m 9 x a x — cos 9 x a y , — sin 9 x s x + cos 9 x s y ) 
9 y = atan'2[—n z , cos 9 x n x + sin 9 x n y ) 


(5.32) 


The pose of the target can be expressed as 


~cam 

X tar 


e 

v 


8* 

8 * 

9, 


Px 

Py 

Pz 


(5.33) 


Equations 5.20-5.29 combined with 5.30-5.32 are nonlinear equations relating the 
image measurements ((x t ,j/,), i = 1 . . . 3) to Xj“™. Due to pixel truncation, the 
measured image positions will not be perfect. These image plane errors will cause 
errors in x'“™. Denote the true image positions as 


x\ = x, + n,- x 


(5.34) 


y'i - Vi + n ty 


(5.35) 


where 


'ft'image — l Tl[ x Tl\ y 


^2x ^2 y ^3r ^3 y 


(5.36) 


is a vector of image noise. Using an approach similar to that of Lee and Kay [29], 
the errors in xf™, arising from n tmage can be found through a linearization 

procedure. The procedure substitutes the true pose, and the 

true image positions, (x', y[), into equations (5.20-5.29). Retaining only the first 
order terms yields 

A/e^ = Dn, mag , (5.37) 
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where 
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(5.38) 


The pose error is then 


v = p z + f 


e“ m = M~ l Dn tmage 


(5.39) 

(5.40) 

(5.41) 


If the exact focal length is not known, then the errors arising in x c t “™ due to the 
focal length error, e/, are (to a first order approximation) 

Xi d x s x 4* Pz 

2/l d\Sy + Py 

-x 2 + d x s x + p x 

1/2 T d x S y T Py 

—Xz + d 2 n x + p x 
—3/3 + d 2 n y + Pz 

Although the errors caused by focal length uncertainties can easily be accommodated 
using (5.42), the present simulation does not include these terms. 


cam 

e tar = 


e f 


(5.42) 


In order to find a set of position and orientation errors corresponding to the 
translations and rotations about the camera frame which will move the measured 
T“ m into alignment with the actual T t c 0 a r m , the coupling between positional errors 
and orientation must be included. This yields the final (camera frame) errors in 
pose due to pixel truncation, ^cam j ^0 be 


^cam 


0 Pz ~Py 

~Pz 0 p x 
Py -Px 0 


M 1 Dn 


image 


= Ln 


image 


( 5 . 43 ) 


The covariance of the vision pose when using the i th algorithm is then 


C„, = L l Cov(n 


image^tmag 


Jif 


( 5 . 44 ) 


where L x is the measurement matrix made from the i viewpoint. 

In order to make use of (5.43), a statistical model must be found for the image 
noise, n tmage . This noise arises strictly from pixel truncation, because the image 
position of a point is only known to be somewhere within the rectangle formed by 
the pixel. With only the knowledge that the position is within bounds, the maximum 
entropy distribution is given by Jaynes MEM principle to be uniform. Consequently, 
a uniform distribution in the image plane is assumed for each pixel. Note that this 
assumption is used strictly in the analysis of accuracy— uniformly distributed noise 
is not added to the image. Instead, the geometry of the pin-hole camera model 
dictates the noise level due to truncation. The variance of the image noise under 
the uniform distribution is 

a l = u £/12 ( 5 - 45 ) 


a 


2 

y 



( 5 . 46 ) 


where w z and w y are the pixel widths in the u x” and ~y” directions, respectively. 
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The image positions of the centers of the retroreflectors is found by finding 
the centroids of the images caused by the retroreflectors. Consequently, the image 
position of the center is much more accurate than a single pixel. This increase in 
accuracy is modeled by assuming that the pixel noise is independent and identically 
distributed between pixels. The covariance of the centroid is then found by 

Cov{n tx ) = ^ ( 5 - 47 ) 

Cov{n ty ) = ( 5 - 48 ) 

where 

• m is the number of rows covered by the image, 

• k x is the number of rows consisting of a single pixel, 

• n is the number of columns covered by the image, and 

• k y is the number of columns consisting of a single pixel. 

Since the centroid is found by summing independent, identically distributed ran- 
dom variables, the distribution of the centroid is (by the Central Limit Theorem) 
Gaussian. This implies that n, mase is a Gaussian random vector, and because e cam 
is linearly related to n image through (5.43), e cam is also Gaussian. 

Once the image measurements have been made, then L may be computed, 
and the error statistics e cam can be found. This information serves as an excel- 
lent performance check after the camera is positioned and measurements have been 
made. Unfortunately, it does not provide a method of evaluating each viewpoint 
without doing the time and computationally intensive re-positioning of the cam- 
era. Fortunately, an estimate of e cam can be obtained without making the image 
measurements based on a. priori knowledge. As the problem statement notes, an a 
priori estimate of the object’s position/orientation in the base frame is known. This 
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frame may be mapped into the i th camera coordinate system using the homogeneous 
coordinate transformation T£j rm which maps frames from the base of the manipu- 
lating arm to the i th camera coordinate system. Because the camera viewpoints 
are pre-specified, the T£' arm transformations can be calculated off-line and stored 
for later use at little cost to the system. Hence the a priori estimate of the three 
points can be transformed from the base frame to each camera frame quite easily. 
Estimates of the error transformation for the i th viewpoint, denoted as Z,, can then 
be calculated. The quality of Z, depends upon the relative error of the a priori 
points in the camera frame. Since the cameras are kept at a respectable distance 
from the object to avoid collisions and interference with the manipulation task, the 
relative error is small, so initial analysis using Z, is quite accurate. If the cameras 
are used in an extremely close proximity to the object, then Z, could conceivably 
be conditioned on the probability density function of the a priori estimates [42] to 
improve the estimate. 

The time which each vision algorithm consumes consists of both a stochastic 
and a deterministic component. First, the cameras must be re-positioned to the i th 
viewpoint. This is a pre-programmed movement, so it takes a deterministic quantity 
of time. Next, the measurement of the points must be made. Suppose that the total 
vision time for the i th algorithm (which includes the camera positioning time, image 
processing time, and the time required for transformation to the end effector frame) 
is 

tvt ~ ( 5 . 49 ) 

Once the pose statistics are found from (5.43) and those of timing are found from 
(5.49), the necessary information for the vision reliability analysis is complete. 

In order to test the validity of the stochastic accuracy model, the vision system 
has been simulated using Pro-MATLAB and the parameters listed in Fig. 5.5. The 
model captures the vision system errors quite nicely over a wide range of operating 
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conditions. Figures B.4-B.6 plot the absolute value of the measurement error as 
the distance from the target to the camera is varied. Also plotted is the 3a bound 
on the measurement error for each direction. The actual transformation from the 
target to the camera frame is 

0 0 1 0.02 
0 1 0 0.02 

T t T= Roily, 0.1) (5.50) 

-10 0 5 

0 0 0 1 

where Rot{y, 0.1) denotes the homogeneous transformation rotating about the u y” 
axis by 0.1 radians with zero translation, and s varies between 0.1 and 3.5meters. 
The accuracy in the l 'x” direction is plotted in Fig. 5.6 for convenience. Note that 
the bound increases as the distance increases because the model incorporates the 
decrease in accuracy as distance is increased. Note also that the “z” error is two 
orders of magnitude larger than the “x” and “y” errors, yet the bound still closely 
follows the actual error. Figure B.7 plots the pose error at each distance weighted 
by the inverse covariance matrix. If the distribution of e cam is zero mean Gaussian 
as assumed and the covariance matrix is correct, then the weighted error will have 
a chi-square distribution with 6 degrees of freedom. Finally, the entropy of the 
vision measurement is plotted in Fig. 5.7 As expected, the entropy increases as the 
distance between the camera and the target increases. 

In order to fully test the strengths and limitations of the statistical model, 
measurements have been taken under a variety of conditions. First, the position 
and orientation of the target is varied. Figures B.8-B.10 are obtained with 

0 0 1 0.1 
0 1 0 0.1 

Rot(z, 0.3) Rot {y, 0.1) (5.51) 

-10 0s 

0 0 0 1 
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Distance from Target [m] 


Figure 5.6: “X” Errors Due To Pixel Truncation 



Figure 5.7: Joint Entropy of the Vision Measurement 
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where i?o<(.r, 0.3) denotes the homogeneous transformation rotating about the "z” 
axis by 0.3. radians with zero translation, and s varies between 0.1 and 3.5m. The 
principle difference between the previous transformation is that the target is shifted 
8cm further off the camera axis in both the “x” and “y”. As expected, the shift 
causes a decrease in accuracy in the “x” and "y" . Fortunately, the statistical model 
captures this phenomena, so the 3 standard deviation bound increases. Naturally, 
information of this sort is very useful for decision making in Intelligent Machines. 

To further test the model, the pose errors are shown in Figures B.11-B.13 
under conditions specifically selected to bring out the unmodeled measurement bias 
caused by approximating the center of the centroid to correspond to the center of 
the retroreflector. This is an approximation which is very often made in computer 
vision algorithms, and is very accurate as long as the object is much farther away 
than one focal length, and the angle between the image plane and the object is 
small. To bring out the effects, the camera is moved very close to the target, and 
the target is rotated sharply. The transformations are 


rncam 

^ tar 


0 0 1 0.1 
0 1 0 0.1 
-10 0s 
0 0 0 1 


Rot(z,0.6)Rot(y, 0.9) 


(5.52) 


where s varies between 2cm and 0.5 meters. Although the errors remain small, the 
3(7 bound is exceeded regularly due to the additional error arising from the centroidal 
bias. This should not introduce any significant problem in practice as long as the 
viewpoints are chosen to avoid these awkward poses. 

To test the sensitivity of the statistical model to uncertainties in parameters 
assumed to be deterministically known, a series of chi-square goodness of fit statis- 
tics have been found as erroneous vision system parameters are used in measurement 
of the target's pose. First, the target length. d\, is perturbed to ±5 % of its nominal 


value. The results of the chi-square test are shown in Figure B.14. The probability 
that the sample would arise from a Gaussian distribution drops off sharply in the 
range of ±1 %. Thus d x should be known to an accuracy of at least ±1 %. The 
target depth, d 2 , displays a similar sensitivity. The results in Figure B.14 indicate 
that d 2 should be known to an accuracy of at least ±2 %. The focal length, on 
the other hand, requires more precise information. As indicated in Figure B.15, the 
focal length should be known to within ±0.2 %. Consequently, it is advisable to 
include focal length uncertainties by using (5.42). 

The simulation results indicate that the stochastic error model for the vision 
system is very good at characterizing the error sources arising from pixel truncation. 
This information will soon be put to use in selecting reliable plans. 

5.2 Analysis of the Kinematics Routines 

In order to act upon the vision system’s perception, it is necessary to use 
an inverse kinematics routine to calculate the joint position corresponding to the 
Cartesian object pose. This step is required because a joint-level computed torque 
algorithm is used for servoing. Several inverse kinematics algorithms have been 
developed, and the choice of which algorithm to use depends upon the time available 
and the accuracy required. 

For this example, five inverse kinematics routines will be considered. The first 
method is probably the most commonly used inverse kinematics technique-simply 
use the nominal link parameters provided by the manufacturer in the standard se- 
rial calculations such as presented in [11]. This algorithm will be denoted by h\. 
The nominal link parameters are not exact due to manufacturing spreads, so error 
is introduced by the using these parameters in the inverse kinematics computation. 
Since many commercially available manipulators are designed explicitly to have 
closed solution inverse kinematics for the nominal link parameters, it will assumed 



that the nominal manipulator possesses a closed form inverse kinematics solution. 
In the simulation, the kinematic model of the Unimation PUMA 560 will be used. 
Unfortunately, if a calibration technique such as that proposed by Hayati [16] is em- 
ployed, the resulting calibrated link parameters may not have closed solution inverse 
kinematics. Consequently, to avoid the time consuming iterative schemes for find- 
ing the inverse kinematics, nominal kinematics may be used even if calibrated link 
parameters are available. To measure the positioning accuracy using A\, the errors 
are measured over a large number of points in the workspace, and the maximum 
entropy Gaussian distribution with the sample mean and variance is applied. This 
technique will be used to assess the accuracy of all of the kinematic routines. For a 
more refined analysis, it is possible to estimate the link parameters as described in 
[16]. Next, through maximum likelihood estimation the positioning error (and its 
distribution) as a function of joint position is estimated [32]. In either case, since 
the nominal kinematics have a closed form solution, the time required to perform 
the computation is fixed. 

The second algorithm, denoted as A' 2 , is very similar to AV The closed form 
solution is retained by calibrating the arm using Hayati’s method, while constraining 
the calibrated link parameters. Since the solution is constrained to be of closed 
form, it results in larger errors than are obtained when the link parameters are not 
artificially inhibited. On the other hand, it shows some increase in accuracy over 
the nominal kinematics. 

The third method, proposed by Hayati [16] and denoted as A' 3 . approximately 
updates the link parameters without iterative schemes by using the inverse ma- 
nipulator Jacobian. Since the Jacobian is employed, the method is ill-conditioned 
in the neighborhoods of Jacobian singular points. Using singular value properties 
and maximum likelihood estimation, an asymptotically normal approximate upper 
bound on the normed positioning error can be found [32]. The bound can be used as 
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a conservative estimate of the positioning accuracy. Although this statistical model 
does capture singularity effects quite nicely, only statistics for the norm (and not 
individual elements) are derived. Since several inverse kinematics algorithms are 
available, the refined estimate is not needed because the other algorithms can be 
employed when in the vicinity of singular points. As a result, accuracy statistics 
obtained by the sample mean and variance (as above) will be used. As in the previ- 
ous algorithms, solution time is stochastic only in that a few conditional loops may 
or may not be executed depending on the particular joint angle. 

The fourth algorithm, denoted as I\ 4 , makes use of Hayati’s calibration tech- 
nique to find the updated link parameters for the particular manipulator under 
consideration. As alluded to previously, the updated kinematics may fail to possess 
a closed form solution, so the iterative Newton— Raphson algorithm will be utilized 
(see [56] for details). Again, this algorithm offers a trade-off: increased accuracy for 
increased time. As before, the errors are measured, and a Gaussian distribution is 
used to model the error. Since the time for executing K 4 depends upon the number 
of iterations required, the execution time is stochastic. To model the statistics, the 
sample mean and variance of the execution time are found, and the most pessimistic 
Gaussian distribution is assumed as a conservative measure. 

The fifth algorithm, A5, also uses the calibrated link parameters. However, in- 
stead of using the Newton-Raphson technique, the Jacobi iterative method outlined 
in [56] is employed. 

To summarize, five inverse kinematics algorithms are available which play time 
against accuracy. The algorithms are: 

I\\ = {Inverse kinematics using nominal link parameters} (5.53) 

A' 2 = {Calibrated kinematics constrained to a closed form} (5.54) 
A3 = {Update kinematics using the manipulator Jacobian} (5.55) 


A\j = {Newton inverse kinematics using updated link parameters} (5.56) 

I\'s = {Jacobi inverse kinematics using updated link parameters} (5.57) 

The choice of inverse kinematics routine depends upon the set of specifications, So- 
The specifications, in turn, interact with the statistical parameters of the algorithm. 
These statistical parameters will be denoted as 

cjt, ~ N{Q,Cki) (5.58) 

tk, ~ N{nt kl ,crt kt ) (5.59) 

where e*.., is the end effector frame Cartesian pose error attributed to the i th inverse 
kinematics algorithm, and is the time required for the inverse kinematics. 

In addition to the inverse kinematic error present in the manipulating arm, 
forward kinematic errors are also present because the pose of the camera arm is not 
known exactly. These forward kinematic errors can be modeled in a very similar 
fashion to the inverse kinematic errors. Through sampled measurements, the co- 
variance matrix of the zero mean forward kinematic error is estimated to be Cfk- 
Lacking any other information, the most conservative distribution is Gaussian. One 
major difference between the forward and inverse kinematic solutions is that the 
forward kinematic solution has a closed form for all joint positions. Consequently, 
rather than having a library of forward kinematic algorithms, it is only necessary to 
have a single forward kinematic algorithm and use the most accurate estimates of 
the link parameters. 

To test the stochastic kinematic models, several tests have been performed. 
First, the normed pose error for three of the inverse kinematics algorithms is plotted 
in Figure 5.8. Note that the Jacobi iterative technique (I\$) produces the smallest 
error, while the Jacobian update method (A' 3 ) yields a small error at most points, 
but actually exceeds the nominal ( I \\ ) errors for three of the fifty random points. 


End Effector Frame P( 
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The normed error and floating point operations required for all inverse kine- 
matic algorithms is illustrated in Figures C.1-C.5. In addition, the 3 a bound on the 
floating point operations is also plotted. As expected, the most accurate algorithms 
are the iterative techniques, but they are also the most computationally intensive. 

To test the assumption that the kinematic errors are Gaussian, the pose error 
for all of the kinematics routines is weighted by the inverse of the covariance matrix 
obtained through the sample statistics (Figures C.6-C.8). Weighting a Gaussian 
random vector by its inverse covariance matrix yields a chi-square distribution with 
degrees of freedom equal to the dimension of the vector. Thus the plots should 
display a chi-square distribution with six degrees of freedom. In addition to the 
weighted norm, Figures C.6-C.8 also plot the median value for the chi-square dis- 
tribution with 6 degrees of freedom (5.6). Thus half of the norms should be above 
this mark, and half below. Chi-square goodness of fit tests indicate that the kine- 
matic algorithms are nicely modeled by a Gaussian distribution with one notable 
exception. The vast majority of the weighted norms for the Jacobian update al- 
gorithm (A' 3 ) are below' the chi-square median. Thus a Gaussian distribution is a 
rather coarse and conservative model for K 3 . 

To test the effect of joint backlash upon the stochastic models, a backlash 
deadband of 0.25 degrees has been added to all joints of the PUMA, and the kine- 
matic models have been found. The weighted norms are plotted in Figures C.9-C.11. 
As before, a zero mean Gaussian distribution fits well for all algorithms except I\ 3 . 

Finally, to examine the effect that joint noise and joint backlash has upon the 
entropy of the kinematic algorithms, the joint entropy of each kinematic algorithm 
is plotted as the noise and backlash is increased (Figures C.12-C.17). As expected, 
the entropy increases as either the joint noise or the backlash is increased. 



5.3 Analysis of the Control System 


To co'ntrol the movements of the manipulator, a computed torque control law 
is used. This control law is based on the Euler-Lagrange dynamic equation: 


r = D(©)0 + NL(Q, 0) 


(5.60) 


where 


• t is the torque applied by the motors, 

• 0 is the joint position vector, 

• © is the joint velocity vector, 

• © is the joint acceleration vector, 

• D(Q) is the inertia matrix, 

• NL(Q, 0) is a vector of torques due to Coriolis, gravity, centripetal, and fric- 
tion nonlinearities. 

An effective method of controlling an arm with dynamics of the form (5.60) is the 
computed torque technique described in [47], where the control function is defined 
as: 

r = D(Q)E (5-61) 

where E is a compensation term. Note that the nonlinear torques due to Coriolis, 
gravity, centripetal and friction effects are not calculated in an open loop fashion 
as is often proposed for computed torque control. Instead, since the concern of this 
study is high precision movements, the movements are made over small distances at 
low velocities. Under these conditions, a PID controller can remove the nonlinear 
disturbances quite effectively. The nonlinear terms can, of course, be calculated if 
a particular application demands the extra level of performance. 
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Since D(0) is invertible due to the physical laws of inertia, this implies that 

s 2 0 = £(s) = G'(s)(0 d (s)-O(s)) (5.62) 

where s denotes Laplace transformation, G(s) is a compensator, and is the 
desired position. The multivariable transfer function for the system is: 

0 = (l/s 2 )[7 + (l/s 2 )^)]- 1 ^)©,, (5.63) 

The joint position, 0, is subject to three sources of error: 

1. Errors in the commanded position 0<*, due to uncertainties in the vision and 
kinematic algorithms. These errors are not affected by the control algorithm 
because a PID control law is used, thus the expected value of the final 0 equals 

Q d . 

2. Errors due to the nonlinear (Coriolis, etc.) disturbances. Because these distur- 
bances are almost constant when moving over small distances at a low velocity, 
the PID compensator can remove these errors when it is operating at steady 
state. The settling time of the closed loop system, therefore, determines the 
time required to remove these effects. 

3. Errors arising from joint position and velocity measurement noise. Although 
the expected value of these errors is zero, the variance depends upon the 
compensator used. 

As the bandwidth of the closed loop system is increased, the settling time 
decreases. Unfortunately, the sensitivity of the system to noise is increased. The 
compensator thus has two conflicting criteria to meet, and the choice of compensator 
depends upon the specifications at hand. For the simulation, two compensators have 
been designed using state space techniques. A continuous time model of the plant 
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(the robot) can be written as: 


x p ~ 


0 I 
0 0 


x p + 


0 -I 


(5.64) 


z — [I 0]x p 4- v (5.65) 

where u is the compensation term (the inverse Laplace transform of E ), v is the 
joint position sensor noise, and 

r e d -e 

6 d -e 

This model assumes that, since D(Q) pre-multiplies E, the inertia term is canceled 
out, and NL(0,0) is removed by the PID compensation at steady state. 

An exact discretization of the plant then yields 


x p — 


(5.66) 


x p {k + 1) = 


/ t.l' 


’ -T 7 

x p (k ) + 


Q tj _ 


: -tj . 


u(k) 


z(k) - [I 0]ip(£) + u(&) 


(5.67) 

(5.68) 


or 


x p {k + l) = A p x p (k) Bpu(k) (5.69) 

z(k) = MpXp(k) + E p v[k ) (5.70) 

where t, is the sampling time. The structure of the discrete PID compensator is 


x e (k + 1 ) = 


0 I 
0 I 


Xc(k) + 


0 

I 


-(*) 


(5.71) 


u(k) = [I\ v /t s - I\ v /t s + K,t,]x c (k) 4- ( I\ v /t s + I\ p + t s I\,)z(k ) (5.72) 


or 


x c (k + 1) — A c x c (k) + B c z(k) 
u(k) = C c x c {k) + D c z(k ) 


(5.73) 

(5.74) 


I 


1 


I 


83 



Figure 5.9: Block Diagram of the Discretized PID Controller 

where the set (A'„. A' p , A',) are the velocity, position, and integral of position gain 
matrices. The closed loop discrete system is illustrated in Figure 5.9 (neglecting 
dynamics due to sampling and a zero order hold). The figure includes process noise, 
u>, which is not used in this study. 

Without any compensation term, the linearized robot plant in Figure 5.9 is 
marginally stable. In order to use the Lyapunov design technique presented in Theo- 
rem 12, the system must be asymptotically stable [27]. The PID compensator makes 
the closed loop system asymptotically stable so that the Lyapunov design technique 
may be employed to yield an optimal control capable of reliably staying within the 
specified hyperellipsoid. In addition, the PID compensator removes positional errors 
due to constant disturbances. 

To ensure reliable control, an outer loop gain is designed using the Lyapunov 
design technique (4.78). The outer loop is found by first formulating the closed loop 
system of Figure 5.9 as 

x(k + 1) = A t x(k ) + D t v t {k) + B t u c {k) (5.75) 

where 

*(*) = [*?(*) x f( fc )] T ( 5 - 76 ) 


] 
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Figure 5.10: Block Diagram with Lyapunov Outer Loop 


At 


A p + B P D C M P B p C c 
B C M P A c 


(5.77) 


Dp BpD c E p 

u t — 

0 B c Ep 

v t (k) = [w T (lc) u T (Ar)] T 



(5.78) 


(5.79) 


(5.80) 


The outer loop is illustrated in Figure 5.10, where is the 24x1 vector of state 
measurement noise. Since the compensator states are known, 


t> 3 = [v T vf 0] 


(5.81) 


where u i is the joint velocity measurement noise. One distinct disadvantage of this 
control scheme is that full state feedback is required. In this case, it means that 
both the joint position and velocity must be measured. The Lyapunov gain, A'/, is 
found by first choosing a positive semi-definite matrix, Qi, and then numerically 


1 


■ 


1 


1 
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solving the algebraic Lyapunov equation 

Q-AjQA t = Q 1 (5.82) 

for Q. Note that, from Lemma 10, Q must satisfy 

Q - Qd > 0 (5.83) 

However, Q is in joint coordinates, while the desired weighting matrix ( Q px ) is in 
Cartesian coordinates. Furthermore, Q px penalizes only Cartesian pose, while Q is 
a weight on all of the state variables (this includes joint position and velocity as well 
as the compensator state variables). Fortunately, it is still a simple matter to find 
an appropriate Q d . First, note that the manipulator Jacobian forms the first order 
term in the Taylor series expansion 

x car( (£) ss x cartd (k) 4- >/(©d( &))[©(£) — ©^(&)] (5.84) 

where x cart {k ) is the actual Cartesian pose at sample k, x cartd {k) is the desired Carte- 
sian pose at sample k , J(-) is the manipulator Jacobian, © d (k) is the joint position 
at sample k, and Q(k) is the actual joint position at sample k. The accuracv of this 
approximation depends, of course, on the magnitude of the difference between the 
desired and actual positions. Since high precision motion control is of concern in 
this study, the difference is very small, so the approximation is very good. By com- 
bining (5.8) and (5.84), the weighting matrix can be projected from the Cartesian 
space into the joint space. Adding zero terms to account for those quantities that 
are not weighted produces the following matrix inequality which must be satisfied 
for all Q d {k) and k in the trajectory 

J T (Qd(k))Q P :(k)J(e d (k)) 0 

> 0 (5.85) 

0 0 

Choosing a Qi which will yield a Q satisfying (5.85) is not straightforward, 
and is a topic of further research. Fortunately, finding a has not posed a problem 
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in this study, although the chosen Q x matrices are undoubtedly conservative. Once 
a satisfactory Q\ has been found, the Lyapunov gain is from (4.78) 

I<, = (BjQB t )- l BfQA t (5.86) 

The final closed loop system is then 

x{k -j- 1 ) = /4j + DtiVi(k) + B t u e {k) (5.8 <) 

where 

Aa = A t - B t I\i (5.88) 

D p B p D c Ep 0 ] 0 BpKu 

D tt = - (5.89) 

0 B C E P 0 J [0 0 

Vi = [w T v T v\\ T (5.90) 

with Ku a matrix of the first twelve columns of A'/. The steady state covariance of 
x, C ct i, is found from (5.87) by solving the discrete Lyapunov equation 

Cdi — AtiCctiAft + DtiCyiDft (5.91) 

where C v i is the covariance matrix of vi. This equation can be easily solved numer- 
ically. 

Two compensators have been developed for use in this study which form a basic 
trade-off between time and accuracy. Both algorithms have a sampling time of t, = 
10ms, have Gaussian joint position noise with standard deviation of 0.167 degrees, 
and Gaussian joint velocity noise with standard deviation 2.4 deg/sec. These large 
standard deviations are used to highlight effects due to noise. In the simulation of 
the combined vision/kinematic/control system, the noise levels are at more realistic 
levels for the PUMA. The joint position noise then has a standard deviation of 0.0033 
degrees, while the joint velocity noise has standard deviation of 0.047 deg/sec. The 
first compensator, denoted by G i, has the following PID gain matrices 

A' v = 15/ (5.92) 


1 


1 


o-a 


] 
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K p = 75/ 

(5.93) 

A', = 125/ 

(5.94) 


By choosing Q\ matrices and evaluating (5.85) over the desired trajectory, it has 
been determined that Q x = 50 / will produce a satisfactory A'/ via (5.82) and (5.86). 
The final closed loop system has a settling time (determined by examining plots of 
step responses) of 3 seconds. 

A second compensator (G 2 ) has been designed by using the PID gains 


> 

c 

II 

o 

(5.95) 

I\ p = 300/ 

(5.96) 

I<i = 1000/ 

(5.97) 


It has been found that Qi — 100/ will produce a Q and A) satisfying the Lyapunov 
matrix inequality (5.85). This system yields a settling time of 1 second. 

The position error for joint 1 of the robot as it moves through the desired 
trajectory is plotted in Figure 5.11. Note that G\ has a longer settling time than 
G' 2 , but it also has a smaller steady state covariance. Thus the choice of which 
algorithm to use depends upon the task at hand. The settling time of the algorithm 
affects the execution time in two ways: 

1. In order to use the reliable control techniques developed in chapter 4, the 
response must be at steady state. Thus the trajectory is scaled so that the 
manipulator is able to achieve a steady state response to the constant Cartesian 
velocity command before a critical region (as defined by the tolerance specifica- 
tions, Figures B.1-B.3) is entered. The constant Cartesian velocity command 
does not. of course, yield a constant joint velocity command. However, since 
the movement is over a small distance and is kept away from manipulator 
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Jacobian singularities, the Jacobian relating joint velocities to Cartesian ve- 
locities changes very little during the movement. The joint velocity command 
is, therefore, almost constant over the fine motion trajectory. 

2. A faster control law requires fewer sampling times in the trajectory. Because 
a joint level control law is used, the inverse kinematics must be solved at each 
sample time. Thus the slower trajectory of a slow compensator requires more 
calculations before movement can begin. 

The steady state covariance, on the other hand, adds to the uncertainty in main- 
taining a desired pose of the gripper with respect to the gripping post. In contrast 
to the pose errors introduced by vision errors and faulty kinematics, which add a 
constant bias error in the commanded pose, the errors arising from joint position 
and velocity noise change as the manipulator moves. In order to apply Theorem 9. 
the steady state value of the pose error inverse covariance matrix must be greater 
than or equal to Q px (Q d (k)). To find the pose error covariance, the joint position 
covariance, C pq , is first determined from C ct i by extracting the position covariance 
terms. Next. C pq is projected into Cartesian space using (5.84) 

C p , = J(O d (k))C pq J(O d (lc)) T (5.98) 

The resulting matrix inequality is 

[J(0 d (/:))C pq 7(0 (i (/:)) r ]- 1 - Q px ( Q d (k)) > 0 (5.99) 

The matrix inequality (5.99) holds for both compensators over the desired trajec- 
tory. Since both (5.85) and (5.99) hold over the trajectory of interest, Theorem 9 
may be employed. In the case study, e 2 = 22. thus \c ( e 2 ) — 0.9988. For a two second 
trajectory. 200 samples are generated (t, = 10ms). therefore the lower bound relia- 
bility of staying within the hyperellipsoid defined by the most restrictive Q px {Od(k)) 
is 


R > (.9988) 200 = 0.79 


(5.100) 



Figure 5.12 plots the weighted square norm of both compensators when using the 
most restrictive weighting matrix ( Q px ) generated by the tolerance specifications. 
The weighted square norm stays well within the hyperellipsoid, because its maximum 
value is on the order of 2, while the hyperellipsoid includes all weighted square norms 
less than or equal to 22. 


5.4 Selection of Feasible Plans 


Because the total number of possible plans for accomplishing the task is 5NxM 
(where N = 10 and M = 2), detailed analysis of every possibility can be very 
time consuming. To avoid a complete analysis for all possibilities, a subset of the 
plans (the feasible plans) will be found which satisfy necessary, but not sufficient, 
conditions for reliable operation as described in chapter 2. 

Consider the individual tasks which must be performed in order to solve the 
given problem. First, the cameras must be moved to one of the N viewpoints. For 
the simulation, ten viewpoints are specified. The initial pose of the camera manip- 
ulator with respect to the nominal target frame is known from a priori information 
to be (see Figure 5.3) 


rncam 
1 tar 


0 0 1 a: 

0 10 y 

-10 0 2 


0 0 0 1 


(5.101) 


where x = 0. y = 0, and z = lm. Each viewpoint is considered to be another vision 
algorithm, so denote the initial viewpoint by Vj. Four more viewpoints (V 2 — V-) 
are found by decreasing z (this means the camera must be moved closer to the 
target) in 20cm decrements. Finally, five more viewpoints (Vg — V l0 ) are found by 
letting x ss 20cm, y = 10cm, and again varying z in 20cm decrements from lm 
to 20cm. For the sake of brevity, it is assumed that the camera manipulator can 





92 



Table 5.1: The Viewpoints Used in the Simulation 


move at 0.2 m/sec from the initial camera position towards other viewpoints. In 
other words, the PUMA 560 dynamics are not included in the camera manipulator 
as a simplification. The dynamics are included in the gripping manipulator, as high 
precision motion is required. The viewpoints, T£“ m , are listed in Table 5.1. 

From the viewpoint, image processing is used to measure the object's position 
in the camera coordinate system. The measurement will, of course, be imperfect 
due to image noise, and it will take a variable length of time due to the iterative 
nature of the solution. The measured pose, T £ e 0 o r ™, is transformed to the end effector 
frame using 

rnbarm rpbarmqrbcam rpcam ^tar i- ino\ 

** grip m ^ beam ** cam m 1 tar m grip 

where the subscript m denotes “measured". Both (the transformation between 

the two manipulator base frames) and T‘“, p (the object grip to target transformation) 
are assumed to be perfectly known as they are fixed. Hence the accuracy of T s 4 “ r p ™ 
depends upon the forward kinematic accuracy of the camera manipulator as well as 
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the accuracy of the vision system. 

A Cartesian space trajectory is then generated which will drive the end effector 
from the initial pose to the grip pose. The trajectory is found by first calculating 
from the known initial end effector pose the grip to end effector frame 


rpce _ rnse rybarm 
1 grip™ 1 barm* gripm 


( 5 . 103 ) 


Because the nominal scenario requires a 20cm movement along the end effector’s 
“z” frame, a set of desired grip to end effector frames is specified to be 


pcc 

9^Pd 


(*) = 


10 0 0 
0 10 0 
0 0 1 P'-T%=i 


0 0 0 


( 5 . 104 ) 


where p, = T**,- (3,4) is the measured distance along the end effector’s l 'z” frame, 

k is the sample number, and N ct i, is the total number of samples used by the i th 
control algorithm. N e ti, is determined from the geometry of the gripper to gripping 
post interface and the settling time of the controller. The control should achieve a 
steady state response to the step velocity command before any fine motion volume 
is entered. For the geometry given in Figures 5.1 and 5.2, the fine motion begins 
when the distance along the end effector’s “z” axis is less than 2 l A + / 3 = 10cm. 
After finding the measured initial distance (p z ), a constant velocity command for 
the i th compensator is found to be 


Pz ~ {-U + h) 


( 5 . 105 ) 


where f je4i is the settling time of the i lh compensator, G,. This choice of v. t allows 
the controller to reach steady state before the fine motion volume is entered. The 
number of samples required for control when using G, is then 


— Pz/ Vzi 


( 5 . 106 ) 
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The number of samples required to achieve steady state is 

f^cths, = ^sei x /^3 (o.lOf) 

The set of desired T'' ipd (k) frames is used to calculate a desired set of end 
effector frames 

(5.108) 

The time required to perform the forward kinematic calculations for a single T^ d m (k) 
is modeled as 

^fk ~ A {n tFK ,<j tF ^) (5.109) 

Based on the desired frames, an inverse kinematics algorithm is selected to 
find a joint trajectory for servoing the manipulator toward, i.e. 

©<(*)- *P£,'"(*)] (5.110) 

where A', specifies the i th inverse kinematics algorithm. The inverse kinematics in- 
troduces stochastic position errors and requires a stochastic period of time. Finally, 
the joint servoing of the motors using the computed torque technique is performed. 

The first specification, Sj, constrains execution time. The total execution time 
for plan .4, = { V’, A',, G,} is 

£. = Ki + -f t kl + t 3 ) (5.111) 

Assuming independence (5.49), (5.59), and (5.109) yield 

£{*>} = Vt, = Mt*. + Kti,(nt k , + Ht F{< + t s ) (5.112) 

Cov{U) = + A; 2 £/i (ct^ k . + a 2 lk> ) (5.113) 

To produce an entropy specification with zero mean, the timing specification will 
be considered as bounds (-[t/ - Ah.M*/ - ht,])- The quantity [t f — ^ £ J is positive 
for reliable systems because otherwise the mean time is less than the maximum 
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execution time, so the resulting entropy from Table 2.1 is = ln(2[i y — p { J). 

The entropy of (5.113) is H(A h ) = In ^J2ve{a^ y -f- A" c 2 (ii ( c? FK + of*,)) As a result, 
the timing constraints will be met if 

In v /2-e(cr i 2 vi + + cr t 2 J) < ln(2(/ / - (5.114) 

is satisfied. 

Using a final time specification of 25sec, and determining the execution time 
by counting the number of floating point operations, the time feasibility analysis has 
been performed. From the original set of 100 plans, 58 of the plans have been selected 
as feasible in meeting the timing specification. The plans selected as feasible are 
intuitively pleasing and are listed in Table 5.2. Each plan in Table 5.2 is expressed as 
a triplet, A = {Vj, A}, G^}. The integer V] denotes the vision algorithm (viewpoint) 
from Table 5.1. The integer A', denotes the inverse kinematics algorithm from (5.53- 
5.57). Finally, the integer G, denotes the compensator used, G\ or G 2 . Note that 
those plans which require large amounts of time are not included. For instance, 
A = {10,4,1}, uses the viewpoint farthest from the initial camera position (V' 10 ), 
the most computationally intensive inverse kinematics routine (A'.,), and the slowest 
compensator (Gi), so it is not included. In fact, no plan which uses the Newton 
iterative inverse kinematics, I\ 4 , is selected as feasible because the algorithm requires 
too many calculations. 

In order to reduce the number of plans considered to be feasible, the ability 
of the plan in meeting the restriction imposed by Lemma 11 is checked at the most 
constraining weighting matrix. Since, from (5.85), Q sub (k) = Q px {Q d [k)). Lemma 
11 implies that the inverse of the pose covariance matrix minus Q px (Q d (k)) must 
be positive semi-definite. As the gripper moves closer to the gripping frame, the 
tolerance to positional errors becomes smaller (see Figures B.1-B.3). This in turn 
causes an increase in Q pI {Qd(k)) in accordance with (5.9). The largest weighting 
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occurs when the trajectory reaches its final point because the end effector and grip- 
ping frames should coincide in the ideal case. Thus Q pr (®d(-V)), where N is the 
final sample, is the most constraining weighting matrix. 

The pose covariance matrix has terms arising from four sources: the vision 
system errors, forward kinematic errors, inverse kinematic errors, and joint control 
errors. In order to propagate the vision system and forward kinematic errors to 
the end effector frame, (5.102) and (5.103) are combined to produce the pose of the 
gripping post with respect to the end effector 

rntt rnte ''pbarm'Y’bcam n^cam qrtar /c | i 

1 gripm 1 barm 1 beam 1 camm 1 tarm 1 grip 

This pose can be represented (excepting kinematic singularity points) as a 6 x 1 
vector by using (5.30)-(5.32). Let 

*= rs.(*?5) (5.116) 

be the nonlinear function mapping the pose of the gripping post in the camera frame 
to the pose of the gripping post in the end effector frame. Then by retaining the 
first order terms in a Taylor series expansion of (5.116), the covariance matrix in 
the end effector frame due to the vision and forward kinematic errors is from (2.14) 
[ 8 ] 

+ C FK )Jtf m {Q d {k)) (5.117) 

where J“ m (j is the interlink Jacobian matrix from the camera frame to the end 
effector frame (see Equation (2.12)). The covariance due to inverse kinematic errors, 
Cki, is measured in the end effector frame, so no transformation is necessary. Finally, 
the control errors in joint space, C pq , are projected to the end effector frame (to first 
order) using (5.98). The total covariance of the gripping post’s pose with respect to 
the end effector for the i th plan is then 

c?rJ‘-ed(k)) = J:ummc„+c FK )jfjeAk))+c t ,+j(6M)c„jmk)) T 

(5.118) 



v; I<, G, 

v; Ki Gi 

v; A, Gi 

v; Ki Gi 

Plan 

4 5 2 

9 5 2 

10 2 1 

10 2 2 

Ri\ 

0.9919 

0.9901 

1.0 

1.0 

I(Rn) 

0.0081 

0.0099 

2.8e-8 

2.2e-16 


Table 5.3: Feasible Plans 


From Theorem 2, if 

c? r :;(i,Od(k))-Q PI mk))> o (5.H9) 

then a lower bound reliability of the i th plan staying inside the specified hyper- 
ellipsoid at sample k can be found. Since the covariance matrices are constants 
multiplied by Jacobian matrices, and the movement is over a small distance away 
from Jacobian singularities, (i, ©<*(&)) is almost constant. Consequently, to 

assess positional feasibility, those plans which are feasible in meeting the timing 
specification are tested to see if 

<%'(*■, e*(0)) - Q PX mN)) > 0 (5.120) 

is satisfied. If (5.120) is true, then the plan is considered to be a feasible plan, and 
further analysis is performed to determine if the plan will execute reliably. The four 
plans deemed to be feasible are listed in Table 5.3. Note that the feasible plans 
require moving the camera much closer to the target (from lm to within 0.4m). In 
addition, note that it is possible to trade the increased inverse kinematic accuracy 
present in the Jacobi iterative technique (A5) for extra accuracy in the vision system 
by moving from viewpoint 9 to viewpoint 10. It is not, on the other hand, possible 
to use A 5 from viewpoint 10 because the available time is too short. 
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5.5 Calculation of Reliability 

The entropy constraints generated in the last section implicitly assure a level 
of reliability. In order to explicitly find the reliability for a particular plan, further 
analysis is warranted. 

First, the specifications (Sp) are used in conjunction with the mathematical 
laws governing the system to produce reliability performance functions correspond- 
ing to each algorithm. The timing RPF for the i th plan follows in a straightforward 
manner: 

<7.i =t; -U (5.121) 

Since t, consists of a linear combination of times, using (5.111) the reliability index 
corresponding to gn can be found from (3.10). The timing reliability index is thus: 

A-i = 

By using a look up table for the zero mean normal cumulative distribution function, 
the reliability can easily be found from (3.11). Moreover, the RSI is found from 
(3.16): 

= -ln$[&i] (5.123) 

Note that all of the reliability self information terms for execution time can be 
determined off-line. This is quite a time saving feature because it allows many 
of the unreliable plans (i.e. plans with large I{R,\) terms) to be identified before 
execution begins. The timing reliabilities and RSI terms for the feasible plans are 
listed in Table 5.3. 

In order to determine which plans are not only feasible, but are in fact reliable 
at meeting the desired set of specifications, So, it must first be ensured that the 
conditions of Lemma 11 are satisfied. For this case study, the conditions become, 


*/ ~ K. + + p tFK ± *«)) 

A ! .. + ^W„ + <) 
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v; K, Gi 

v; K, Gi 

Ki G, | 

Plan 

4 5 2 

9 5 2 

10 2 1 1 

Time Relia. 

0.9919 

0.9901 

1.0 j 

Time RSI 

0.0081 

0.0099 

2.8e-8 | 

Accuracy Relia. 

0.8555 

0.8555 

0.6261 

Accuracy RSI 

0.1561 

0.1561 

0.4682 

Overall Relia. 

0.8486 

0.8470 

0.6261 1 

Overall RSI 

0.1642 

0.1660 

0.4682 | 


Table 5.4: Plans Satisfying the Matrix Inequalities 


from (5.85) and (5.119) 


Q - 


J T (Q d (k))Q px (e d (k))J(Q d (k}) 0 


0 


> 0 


c e X(i,e d (k))-Q p ,(©,(*)) >0 


(5.124) 

(5.125) 


These matrix inequalities must be satisfied for all of the desired joint positions in 
the fine motion portion of the trajectory, 0<*(fc), k = N ct i 3 j l , • • ■ , N ct i, . Since the 
fine motion requires between 100 and 400 samples, this reliability check is quite 
computationally intensive. However, the fine motion joint trajectory is over a very 
small range. As a consequence, a smaller number of samples can be checked as a 
very good approximation. In the simulation, 20 sample points evenly distributed in 
the fine motion trajectory are substituted into (5.124) and (5.125). Of the feasible 
plans, the plans listed in Table 5.4 satisfy the matrix inequalities at all 20 sample 
points. Since the reliable plans meet the conditions of Theorem 9, it is very easy 
to find a lower bound reliability of meeting the accuracy specification, s 2 - Thus, for 
a single sample, e 2 = 22, Rk > X6(f 2 ) = 0.9988. The probability of Af eaJt staying 
within $2 during fine motion control has a lower bound of 


R, 2 > [0.9988] A ' e,i ‘ 


(5.126) 
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The RSI is 

I[Ra) = -(Mcti, - Ncttss,) ln(xg(22)) (5.127) 

Equations (5.123) and (5.127) are explicit functions relating the i th feasible subset 
of algorithms, Aj eaSi , and the environment and design stochastic variables to the 
RSI. The equations must now be combined to form one total RSI for each subset of 
algorithms. 

Since all of the specifications must be satisfied to ensure that the desired 
standards are met and the specifications are independent, the system forms a series 
connection. The total RSI for a particular subset of algorithms, A/ ea , t , is found 
from (3.20) to be the sum of the RSI’s for each specification, i.e. 

I(R,) = I(Rn) + I(R l2 ) (5.128) 

A threshold can be set on /(/?,) to discriminate reliable from unreliable plans. Those 
plans with a total reliability self information less than the threshold will then be 
considered reliable. In this manner, a specified level of reliability in meeting the 
desired requirements can be achieved. For instance, suppose a reliability of 0.75 is 
desired. The RSI threshold is then 0.2877. From Table 5.4, the RSI for .4 = (10,2, 1) 
is 0.4682. thus .4 = (10,2, 1) is not considered to be a reliable plan. On the other 
hand, both plans (4,5,2) and (9,5,2) have acceptable overall RSI values and will 
therefore execute reliably. 

5.6 Comments 

To test the results of the analysis, several of the plans have been simulated 
using the vision system depicted in Figures 5.4 and 5.5, PUMA 560 kinematics for 
both the camera and end effector manipulator, and PUMA 560 dynamics for the 
end effector manipulator. The pose errors for plan (4,5,2) are plotted in Figures 
5.13-5.15. In addition, the upper and lower tolerance bounds from Figures B.1-B.3 



are also plotted, as dashed and dotted lines, respectively. The errors in all directions 
stay well within the tolerance bounds, and the plan is executed within the timing 
constraint, so the plan is successful in achieving the task at hand. Similar results are 
obtained for the second reliable plan, (9,5,2). The results are illustrated in Figures 
D.1-D.3. To check repeatability, the plans are repeatedly tested. Both plans have 
reliably executed the entire task twenty consecutive times. 

In contrast, A = (1, 1, 1) is not selected by the analysis as a reliable plan. The 
results of this plan are depicted in Figures D.4-D.6. In this case, both the "Y” and 
“Z” errors exceed the tolerances. 

As these examples illustrate, the reliability analysis techniques developed herein 
are very effective at selecting reliable plans from a set of plans. In addition, a means 
is illustrated whereby the positioning tolerances arising from the geometry of a 
gripping interface can be recast as a position-varying hyperellipsoid specification. 
Since the case study example is a very high fidelity simulation of realistic multi-arm 
robotic systems, the algorithms analyzed are fully capable of being implemented 
on standard robotic hardware. Thus the results of the case study may be directly 
applied to actual systems. 


X Error (mj, Plan=|4 5 2] X Orientation Error (deg], Plan=|4 5 2] 
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Figure 5.13: Error and Tolerance Bounds for the “X” Axis, Plan (4,5,2) 
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6. CONCLUSIONS AND FUTURE WORK 


By bringing together concepts from reliability and information theory, a new tech- 
nique tor systematically selecting reliable designs capable of executing a given task 
has been developed. The two stage procedure has several advantages. In particu- 
lar, because the first stage proposes a coarse but computationally efficient entropy 
based elimination algorithm, a large number of unsuitable plans can quickly be ex- 
cluded from further analysis. For scalar random variables subject to tolerance spec- 
ifications, a lower bound on the reliability imposed by satisfaction of the entropy 
constraints is derived as long as the distribution of the random variable is either 
Gaussian, uniform, double exponential, Poisson, or Rayleigh. A lower bound relia- 
bility is also derived for correlated zero mean Gaussian vectors subject to quadratic 
specifications. The approach is especially adept at analyzing physically distributed 
systems due to the invariance of joint entropy to homogeneous coordinate frame 
transformations. 

The second stage provides a practical framework for explicitly calculating the 
probability of success of a feasible plan. In this manner, plans of sufficient reliability 
can be found. Two methods are proposed for calculating the probability. Maximum 
likelihood estimation is suggested because it possesses the property of asymptotic 
normality. A large number of samples is required, of course, for asymptotic normal- 
ity to be safely assumed. The first order, second moment methods from structural 
reliability offer a second alternative for calculating the probability of success. Lower 
bounds on reliability can be found even for nonlinear reliability performance func- 
tions. The reliabilities calculated by either technique can be combined to find the 
probability of meeting all of the specifications simultaneously as long as the speci- 
fications are independent. Plans of sufficient reliability may then be executed. 

To allow analysis of performance over a time interval, a lower bound on the 
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reliability of a discrete time-invariant control in meeting a set of quadratic speci- 
fications is 'derived. This result can easily be applied to robotic systems after the 
system is linearized using nonlinear feedback. The quadratic specifications delineate 
the maximum acceptable value of a weighted square I 2 norm of the system’s state 
error. The state error must be a zero mean Gaussian vector, so the measurement 
and process noise must be zero mean Gaussian variates. Although the analysis 
is conservative because it provides only a lower bound on the reliability, it does 
produce useful results in the case study. 

The case study of visual positioning successfully applies these new techniques 
to a problem of practical significance. Because the errors in visual sensing, kinematic 
transformations, and control can all be accommodated in the analysis framework, an 
unprecedented level of statistical modeling for visual positioning has been achieved. 
Given realistic dimensions for a gripper and a gripping post, actual camera parame- 
ters, and the kinematic and dynamic models for the PUMA 560, the analysis deemed 
3 out of 100 plans to be reliable. Each of the reliable plans did, in fact, execute 
reliably. Plans which were not deemed reliable also performed as expected, as they 
exhibited failures. This statistical model should prove very useful in high precision 
robotic tasks such as VLSI etching, medical manipulators, micro-robotics, etc. 

6.1 Contributions 

The contributions of this work are the following: 

• A new method of representing specifications which delineates the maximum 
level of uncertainty allowed by the specification has been suggested. 

• A new approach to reliability analysis based upon satisfaction of a set of 
entropy constraints has been developed. 


• An original application of entropy invariance to homogeneous coordinate frame 
transformations is proposed for simplifying analysis of distributed systems. 

• A systematic framework has been developed for calculating reliability of robotic 
systems. 

• A lower bound on the reliability of a zero mean Gaussian vector meeting a 
quadratic specification is derived. 

• The reliability of a control/sensing system over a time interval has been evalu- 
ated by deriving lower bounds on reliability for discrete time-invariant control 
systems. 

• The first complete statistical analysis of visual positioning in the full six degree 
of freedom case has been developed. Errors arising from image noise, forward 
kinematic transformations, inverse kinematic transformations, and joint mea- 
surement noise are stochastically modeled. 

6.2 Future Work 

To extend the research presented in this thesis, the following issues may be 
addressed: 

• The reliability analysis finds the probability of success in achieving a set of 
specifications, but does not assign a cost to alternative strategies. Decision 
making may be enhanced by adding cost criteria. 

• Rather than visual positioning, the case study can be extended to visual ser- 
voing. This will desensitize the closed loop system to forward and inverse 
kinematic errors if a Cartesian control law is used. 

• Methods for analyzing digital controls with assorted update times (i.e. a fast 
sampling time for closed loop control, but a slower sampling rate available 
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from a visual measurement system) need to be developed. 

• Some applications may mandate a small number of samples when estimating 
statistics for individual algorithms. The small sample properties and the levels 
of confidence arising from the estimates can be investigated. 

• An analysis of the reliability when discrete distributions are used may offer 
some advantages. First, since the actual distributions are discrete due to the 
presence of analog to digital converters, the modeling accuracy may increase. 
Second, many entropy properties have been developed in information theory 
for discrete distributions, so these properties may be employed. 

• It might be possible to blend the results of the control analysis with Skelton’s 
covariance control to yield a combined analysis/design technique. 

• The reliability of other sensing algorithms can be analyzed and incorporated 
into the framework. 

In conclusion, the framework for reliability analysis presented seems to be 
effective at selecting reliable plans for robotic systems. The case study example is a 
very high fidelity simulation of realistic multi-arm robotic systems. Moreover, the 
algorithms developed are fully capable of being implemented on standard robotic 
hardware. 
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APPENDIX A 

IMPLICIT RELIABILITY FOR TOLERANCE SPECIFICATIONS 

For a tolerance range represented as /x r ± d, 


R = P{ \x-» x \<d) 

(A.l) 

The entropy constraint requires that 


H{x) < In 2 d 

(A.2) 

For a class of distributions including the Gaussian, uniform, 

double exponential, 

exponential, Poisson, and Rayleigh 


H(x) = In a T 4- In B 

(A.3) 

where B is a constant. This implies that 


In a x + In B < In 2d 

(A.4) 

Zl < — 

(A. 5) 

d ~ B 

From the Chebyshev inequality, 


R = P{ \x - fi x \ < d) > 1 - al/d 2 

(A.6) 


or 

R>l-4/B 2 (A.7) 

Thus a conservative lower bound on reliability implicitly guaranteed by the entropy 
constraints (for tolerance specifications) can be found for a large class of distributions 
by determining the constant, B , for the particular distribution. 
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APPENDIX B 

VISION SYSTEM SIMULATION RESULTS 


Simulation results for the Marshall Space Flight Center vision system are presented 
in this appendix. Figures B.1-B.3 display the tolerance bounds of the gripping 
interface as the distance between the two frames is varied. Figures B.4-B.6 show 
the actual measurement errors of the vision system as well as the 3 <7 bound predicted 
by the stochastic model while the pose of the target is kept at a typical value. Figure 
B.7 plots the normed pose error as it is weighted by the predicted inverse covariance. 
If the distribution is zero mean Gaussian as assumed and the predicted covariance 
is correct, then the plot should have a chi-square distribution with 6 degrees of 
freedom. 

To test the robustness of the stochastic model for the vision system, Figures 
B.8-B.13 plot the measurement errors and 3cr bound for two poses which violate the 
assumptions of the statistical model. The first test places the pose of the target at 
an extreme angle so that the cross sectional area of the retroreflector in the image 
plane is very small. The second test purposely moves into positions which are not 
likely in a practical environment, but which induce a large bias due to the centroid 
not corresponding to the center of the retroreflector. Finally, Figures B.14 and B.15 
provide the chi-square goodness of fit probability as the target parameters d x and 
d 2 as well as the focal length are varied. 
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Figure B.6: Nominal Case-“Y” and “Z” Orientation Errors 


1 





Camera Frame Y error in meters Camera Frame X enor in meters 


0.012 


0.01 b 


Absolute Value of Measurement Error 
• 3 Standard Deviation Bound 


0.008 b 


0.006 b 


0.004 b 


0.002 



1.5 2 2.5 

Distance from Target [m] 



) i ^ i : i : i — __i~ » ! 

0 0.5 1 1.5 2 2.5 3 3.5 


Distance from Target [m] 


Figure B.8: Offset Case-“X” and “Y” Errors 
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Figure B.9: Offset Case-“Z” Position and “X” Orientation Errors 
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Figure B.13: Centroidal Bias Case— “Y” and “Z” 
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APPENDIX C 

KINEMATIC SIMULATION RESULTS 


Appendix C provides the kinematic simulation results. Figures C.1-C.5 plot the 
/ 2 norm of the end effector frame pose error for each of the five inverse kinematic 
routines. The errors are found at a number of randomly selected joint positions, 
with the same set of joint positions used for each inverse kinematic algorithm. In 
addition, the floating point operations required to execute each of the algorithms is 
also displayed, along with the sample mean and 3a bound. 

Figures C.6-C.8 plot the norm squared end effector pose error as it is weighted 
by the predicted inverse covariance matrix. The joint noise and backlash are kept at 
values normally expected for the PUMA 560. The resulting distribution should be 

so the median of the chi-square distribution is also plotted (it is 5.6). Figures 
C.9-C.11 are very similar to the previous plots, but in this case a large backlash 
deadband is added. The statistical properties do not greatly change. 

Figures C.12-C.17 plot the joint entropy of the end effector frame pose error 
for each kinematic algorithm as two statistical parameters are varied. First, the 
standard deviation of the joint noise is increased from 0 to 0.35 degrees. As expected, 
the entropy also increases. Finally, the width of the backlash deadband is varied 
from 0 to 2 degrees as the entropy is plotted. 
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Figure C.l: Normed Error for the Nominal Inverse Kinematics Routines 
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Figure C.3: Jacobi Normed Error and Nominal FLOPS 
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Figure C.8: A'., and I\ 5 Weighted Norms 
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Figure C.12: Entropy of I\\ and I\ 2 with Joint Noise 
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Figure C.13: Entropy of I\ 3 and I\ 4 with Joint Noise 
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Figure C.16: Entropy of A' 3 and A' 4 with Backlash 
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APPENDIX D 

PLAN EXECUTION SIMULATION RESULTS 


This appendix plots the pose errors in each direction as the gripping frame is ap- 
proached. The results for two plans are presented. First, plan A = {9,5,2} is 
simulated. Since it uses vision algorithm 9, the camera is 0.4m from the target 
when the picture is taken. Additionally, I\ 5 (the Jacobi interative inverse kinemat- 
ics method) is used, so errors due to the inverse kinematics are minimized. Finally, 
G 2 is used as a compensator. Since it has a short settling time, it allows the tra- 
jectory to be traversed quickly. The errors are plotted in Figures D.1-D.3, along 
with the tolerance bounds obtained from the geometry* of the gripping interface. 
The errors are well within the tolerance bounds throughout the entire trajectory. 
This is the expected result because A = {9,5,2} has been validated by the analysis 
procedure to be a reliable plan. 

Plan A = {l, 1, 1}, on the other hand, is not deemed to be a reliable plan. The 
errors and tolerance bounds for A = {l, 1, 1} are plotted in Figures D.4-D.6. The 
plan does not meet the specifications. In fact, the tolerance bounds are exceeded in 
both the "V" and “Z” directions. 
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Figure D.5: Error and To 




Z Error (m], Plan=[l I I] Z Orientation Error [deg], Plan=( I I 1] 


156 




Figure D.6: Error and Tolerance Bounds for the “Z” Axis, Plan (1,1,1) 






